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内 容 简 介 


目前 同类 图 书 大 多 侧重 于 对 机 器 人 控制 相关 理论 知识 的 介绍 和 研究 ,比较 抽象 ,同时 也 难以 培养 读者 的 
动手 能 力 , 因 此 难以 取得 良好 的 实践 效果 。 本 书 注重 理论 与 应 用 的 结合 ,力求 使 读者 能 够 尽快 掌握 机 器 人 控 
制 技术 ,了 解 其 主要 研究 方向 。 

全 书 分 为 三 篇 ,分 别 介绍 机 器 人 学 相关 的 基础 理论 知识 、 常 用 控制 方法 ,以 及 作者 在 机 器 人 控制 领域 的 最 
新 研究 成 果 ,阐述 机 器 人 控制 技术 在 人 机 交互 方面 的 应 用 。 ” 

本 书 可 作为 高 校 自 动 控制 .计算 机 相关 专业 的 高 年 级 本 科 生 、 研 究 生 以 及 高 校 教师 的 教材 或 教 辅 ,同时 也 
可 作为 机 器 人 研发 人 员 和 相关 工程 技术 人 员 的 参考 用 书 。 
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机 器 人 学 代表 了 当今 集成 度 高 .具有 代表 性 的 高 技术 领域 , 它 体现 了 机 械 工程 学 .计算 机 
技术 ,控制 工程 学 .电子 学 .生物 学 等 多 学 科 的 交叉 与 融合 ,代表 了 当今 实用 科学 技术 的 先进 水 
平 。 一 般 而 言 , 机 器 人 由 机 械 部 分 (一 般 是 指 通过 各 关节 相连 组 成 的 机 械 臂 ) . 传 感 部 分 (包括 
测量 位 置 .速度 等 的 测量 装置 ) ,以 及 控制 部 分 (对 传 感 部 分 传 来 的 测量 信号 进行 处 理 并 给 出 相 
应 控制 作用 ?组 成 。 

机 器 人 控制 技术 作为 机 器 人 的 “大 脑 ”, 主 要 是 根据 传感器 等 传送 的 信息 ,采用 控制 算法 ， 
使 得 机 械 部 分 完成 目标 操作 而 承担 相应 控制 功能 对 应 的 部 分 。 其 最 终 目的 是 尽 可 能 减 小 机 器 
人 实际 运动 轨迹 与 期 望 目标 的 偏差 ,达到 理想 的 运动 精度 。 随 着 机 器 人 相关 技术 的 演进 ,控制 
算法 也 逐渐 变 得 更 丰富 ,产生 了 诸如 自 适应 控制 、. 自 校正 控制 , 鲁 棱 控制 、 变 结构 控制 、 非 线性 
系统 控制 预测 控制 等 众多 新 型 控制 策略 。 

本 书 是 作者 在 机 器 人 控制 人 机 交互 、 人 机 技能 传递 , 遥 操 作 和 示 教 技术 方向 多 年 的 研究 
和 教学 工作 的 积累 。 作 者 在 柔性 机 械 壁 仿生 控制 、 肌 电信 号 处 理 、 变 刚度 控制 人 机 技能 拓展 
等 领域 的 理论 和 应 用 研究 ,取得 了 一 批 领先 的 理论 成 果 和 技术 成 果 。 本 书 由 浅 入 深 地 讲述 机 
器 人 控制 技术 的 基础 .方法 和 应 用 ,将 理论 和 实际 应 用 紧密 结合 。 书 中 针对 不 同 的 控制 方法 给 
出 了 具体 应 用 实例 ,这 些 实例 的 应 用 场景 前 沿 且 热 门 。 本 书 不 仅 详细 阐述 了 控制 器 的 设计 过 
程 ,还 给 出 了 具体 的 仿真 /实验 过 程 和 结果 分 析 。 本 书 从 理论 到 应 用 , 层 层 递 进 ,易于 理解 。 

书 中 介绍 的 基础 知识 适合 高 年 级 本 科 生 学 习 , 关 于 机 器 人 控制 方法 ( 即 控制 器 的 设计 ) 和 
机 器 人 控制 技术 的 应 用 则 适合 研究 生 学 习 研 究 。 同 时 ,本 书 也 适合 与 机 器 人 控制 方向 相关 的 
科研 人 员 学 习 参 考 。 
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本 书 将 理论 与 实际 相 结合 ,完整 地 阐述 从 基础 知识 到 应 用 实践 的 机 器 人 控制 理论 。 初 学 

者 可 以 从 中 学 习 机 器 人 控制 的 基础 知识 ,而 相关 领域 的 科研 人 员 可 以 通过 这 本 书 接触 到 当前 
机 器 人 控制 的 先进 技术 ,因而 适合 不 同 层次 的 机 器 人 爱好 者 学 习 。 
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本 书 是 一 部 机 器 人 控制 领域 实用 且 前 沿 的 图 书 。 书 中 不 仅 有 机 器 人 控制 相关 的 基本 理 

论 , 还 结合 作者 在 该 方向 的 最 新 研究 成 果 进 行 更 深入 的 阐述 ,由 浅 和 人 深 ,内 容 翔 实 ,是 一 本 难得 
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本 书 从 基础 、 理 论 和 应 用 三 个 方面 , 层 层 递 进 , 详 细 地 介绍 了 机 器 人 控制 器 的 设计 方法 及 
其 应 用 实例 , 极 具 实用 性 和 参考 价值 。 
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本 书 内 容 详尽 ,涵盖 了 机 器 人 控制 领域 常用 且 主 流 的 控制 方法 ,融合 了 作者 在 人 机 交互 领 

域 多 年 研究 的 心得 ,是 一 本 机 器 人 控制 方面 的 指导 性 著作 ,具有 很 高 的 参考 价值 。 
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本 书 系统 而 深入 地 介绍 了 机 器 人 控制 方向 的 先进 技术 ,方法 以 及 实际 应 用 。 内 容 新 颖 , 注 
重 理论 与 实际 应 用 相 结合 ,方便 读者 理解 和 掌握 抽象 难 懂 的 控制 理论 及 其 实现 过 程 。 
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这 本 关于 机 器 人 控制 技术 的 著作 ,不 仅 适合 机 器 人 控制 领域 的 初学 者 ,也 适合 科研 人 员 研 
究 学习 。 书 中 集成 了 作者 在 机 器 人 控制 方向 的 诸多 研究 成 果 , 从 基本 理论 ,方法 到 具体 实例 ， 
内 容 丰 富 ,研究 方法 新 颖 主流 ,是 机 器 人 控制 和 人 机 交互 领域 难得 的 图 书 。 
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随 着 “工业 4.0” 时 代 的 来 临 , 以 机 器 人 、 智 能 制造 蔡 代 部 分 人 工 已 成 为 时 代 趋 势 。 机 器 
人 控制 器 是 根据 指令 以 及 传 感 信 息 控制 机 器 人 完成 一 定 的 动作 或 作业 任务 的 装置 , 它 是 机 器 
人 的 心脏 ,决定 了 机 器 人 性 能 的 优 劣 。 目 前 ,由 于 人 工 智 能 .计算 机 科学 传感器 技术 及 其 他 相 
关 学 科 的 长 足 进步 ,使 得 机 器 人 技术 的 研究 在 高 水 平 上 进行 ,同时 也 对 机 器 人 控制 器 的 性 能 提 
出 更 高 的 要 求 , 对 于 不 同类 型 的 机 器 人 ,如 双 足 步行 机 器 人 与 关节 型 工业 机 器 人 ,控制 系统 的 
综合 方法 有 较 大 差别 ,控制 器 的 设计 方案 也 不 一 样 。 

传统 的 机 器 人 控制 领域 的 图 书 大 多 侧重 于 对 机 器 人 控制 理论 知识 的 介绍 和 研究 ,对 读者 
而 言 比 较 抽 象 。 这 些 知 识 难以 培养 学 生 的 动手 能 力 , 难 以 取得 良好 的 实践 教学 效果 。 本 书 注 
重 理论 与 应 用 的 结合 ,力求 使 读者 能 够 尽快 掌握 机 器 人 控制 技术 ,了 解 机 器 人 控制 领域 一 些 比 
较 主 流 的 研究 方向 。 本 书 将 机 器 人 控制 的 理论 和 应 用 相 结合 ,一 方面 概要 地 介绍 了 机 器 人 控 
制 的 理论 , 另 一 方面 着 力 于 介绍 机 器 人 控制 方法 的 应 用 实例 。 

本 书 大 部 分 内 容 以 各 种 机 械 臂 为 被 控 对 象 , 书 中 介绍 的 控制 方法 大 多 选 自 作者 近 几 年 发 
表 在 国际 高 水 平 杂 志和 会 议 中 的 论文 ,是 近 几 年 机 器 人 控制 领域 比较 先进 或 者 主流 的 控制 方 
法 。 书 中 还 有 一 些 控 制 方法 选 自 机 器 人 控制 领域 相关 书籍 中 的 典型 方法 ,并 在 不 同 程度 上 对 
这 些 方 法 进行 了 改进 和 补充 。 在 具体 应 用 场景 中 ,通过 对 不 同 控制 器 设计 方法 详细 的 理论 阐 
述 和 仿真 /实验 结果 分 析 ,使 抽象 难 懂 的 控制 理论 易于 理解 和 掌握 ,方便 感 兴趣 的 读者 进行 更 
深入 的 研究 。 

全 书 内 容 分 为 三 篇 。 第 一 篇 介绍 机 器 人 学 相关 基础 理论 知识 ,包括 机 器 人 运动 学 机 器 人 
动力 学 .机 器 人 轨迹 规划 等 ,这 些 知识 是 后 续 机 器 人 控制 器 设计 、 机 器 人 动力 学 和 运动 学 建 模 
的 基础 。 第 二 篇 介绍 机 器 人 控制 方法 ,如 自 适应 控制 .神经 网 络 控制 . 变 结构 控制 、 滑 模 控制 
等 。 针 对 常用 的 一 些 控制 方法 , 书 中 都 有 详细 的 理论 介绍 ,对 于 其 他 控制 方法 , 书 中 也 有 简单 
的 概述 。 对 于 每 种 控制 方法 ,以 作者 相关 科研 成 果 为 基础 ,都 给 出 了 有 具体 的 控制 器 设计 实例 和 
仿真 结果 。 第 三 篇 结合 作者 在 机 器 人 控制 领域 的 最 新 研究 成 果 , 阐 述 机 器 人 控制 技术 在 人 机 
交互 方面 的 典型 应 用 。 例 如 人 机 交互 . 遥 操 作 技 术 、 机 器 人 示 教 等 ,这 些 都 用 到 了 机 器 人 控制 
技术 ,或 者 说 在 某 种 程度 上 ,它们 都 是 以 机 器 人 控制 技术 为 基础 进行 的 。 另 外 , 书 中 介绍 的 一 
些 具体 实例 大 都 是 基于 不 同 的 应 用 场景 ,对 此 , 书 中 也 有 一 些 简单 的 背景 概述 ,对 此 应 用 场景 
感 兴趣 或 者 想 要 进行 更 深入 了 解 的 读者 可 以 参考 相关 领域 更 专业 、 更 全 面 的 书籍 资料 。 

本 书 是 在 总 结 作者 多 年 科研 成 果 的 基础 上 撰写 而 成 ,不 但 可 作为 高 校 机 器 人 控制 相关 专 
业 高 年 级 本 科 生 硕士 生 或 者 博士 生 , 以 及 高 校 教师 的 教材 或 辅导 材料 ,同时 也 可 作为 机 器 人 
研发 人 员 和 相关 工程 技术 人 员 的 参考 书 。 本 书 第 一 篇 和 第 二 篇 理论 部 分 的 内 容 适 用 于 本 科 生 
进行 理论 学 习 , 第 二 篇 中 针对 每 种 控制 方法 介绍 的 具体 应 用 实例 和 第 三 篇 的 内 容 可 作为 研究 
生 及 其 他 科研 人 员 进 行 科研 或 者 实际 应 用 的 参考 。 

感谢 在 本 书 编写 过 程 中 ,牺牲 宝贵 时 间 协 助 我 们 完成 这 本 书 的 实验 室 的 学 生 ,感谢 他 们 对 
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书稿 提出 的 宝贵 意见 。 其 中 , 彭 光 柱 博士 参与 了 本 书 8.5 节 、9.4 节 和 9.5 节 内 容 的 编写 , 研 
究 生 黎 伟 豪 、 吴 怀 伟 、 徐 艳 宾 、 王 行 健 . 许 扬 分 别 参与 了 9.7 节 、11.1 节 、11. 2 节 、11.7 节 和 
13. 2 节 内 容 的 编写 工作 。 

本 书 参考 了 国内 外 学 者 的 大 量 论文 和 专著 ,由 于 篇 幅 有 限 , 书 中 未 能 详尽 列 出 ,说 在 此 表 
示 庄 心 感谢 。 

由 于 作者 水 平 有 限 , 书 中 难免 存在 错误 和 不 妥 之 处 , 敬 请 读者 给 予 批评 指正 。 


学 习 说 明 


本 书 共 分 为 三 篇 ,前 两 篇 关联 性 较 强 ; 第 三 篇 侧重 于 应 用 ,相对 独立 。 以 下 分 别 对 每 一 篇 
的 内 容 进行 梳理 ,并 给 出 一 些 学 习 建 议 。 

第 一 篇 : 基础 篇 ,介绍 了 机 器 人 学 中 的 数学 基础 .机 器 人 运动 学 、 机 器 人 动力 学 、 机 器 人 控 
制 和 轨迹 规划 。 建 议 读者 在 学 习 该 部 分 时 ,应 具备 一 定 的 机 器 人 学 基础 。 第 二 篇 中 ,机 器 人 控 
制 器 设计 方法 是 以 这 部 分 内 容 为 基础 ,尤其 对 于 本 篇 中 关于 机 器 人 运动 学 和 动力 学 建 模 部 分 
的 内 容 , 最 好 能 够 理解 并 掌握 。 

第 二 篇 : 方法 篇 ,介绍 了 不 同 的 机 器 人 控制 器 的 设计 方法 和 应 用 实例 。 本 篇 介绍 的 控制 
方法 大 都 比较 前 沿 ,读者 在 阅读 本 篇 内 容 时 ,应 根据 书 中 介绍 的 方法 ,推导 机 器 人 控制 器 的 理 
论 公 式 。 对 于 本 书 给 出 的 大 多 应 用 实例 , 感 兴趣 的 读者 可 以 参考 书 中 的 方法 设计 相应 的 控制 
器 并 进行 仿真 。 有 条 件 或 者 实验 平台 的 读者 可 以 在 仿真 的 基础 上 进行 实验 。 另 外 , 书 中 部 分 
应 用 实例 涉及 的 一 些 软件 ,如 V-REP、MATLAB 等 ,建议 初学 者 参考 (4 机 器 人 仿真 与 编程 技 
术 》(ISBN 为 978 7 302 4904 87, 由 清华 大 学 出 版 社 于 2018 年 2 月 出 版 ) 进 行 学 习 。 

第 三 篇 : 应 用 篇 ,介绍 了 机 器 人 控制 技术 中 比较 主流 的 应 用 。 本 篇 内 容 在 理论 上 和 前 两 
篇 的 联系 不 是 很 强 , 读 者 可 以 将 其 看 作 是 对 机 器 人 控制 技术 应 用 的 拓展 。 当 然 , 对 这 些 应 用 方 
向 感 兴趣 的 读者 ,可 以 参考 书 中 的 应 用 实例 进行 学 习 和 仿真 ,并 在 此 基础 上 进行 深入 研究 。 
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本 篇 主要 介绍 机 器 人 学 相关 的 基础 理论 知识 ,包括 机 器 人 学 中 的 数学 基础 .机 器 人 运动 
学 .机 器 人 动力 学 、 机 器 人 雅 可 比 矩 阵 、 机 器 人 轨迹 规划 、 机 器 人 力 /位 置 控制 等 ,作为 后 续 机 器 
人 控制 设计 与 应 用 的 基础 知识 的 补充 。 
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机 器 人 学 数学 基础 


CHAPTER 1 


机 器 人 可 以 看 作 是 由 一 系列 连 杆 通过 关节 组 成 的 刚体 。 通 常 来 说 ,机 器 人 指 的 是 至 少 包 
含有 一 个 固定 的 刚体 和 一 个 活动 的 刚体 的 物体 。 其 中 ,固定 的 刚体 称 为 基 座 ,而 活动 的 刚体 则 
称 为 末端 执行 器 。 在 两 个 部 件 间 会 有 若干 连 杆 和 关节 来 支撑 末端 执行 器 ,并 使 其 移动 到 一 定 
的 位 置 。 描 述 机 器 人 各 个 连 杆 之 间 、 机 器 人 与 其 作业 对 象 的 相互 运动 关系 ,实际 上 就 是 描述 刚 
体 之 间 的 运动 关系 。 在 研究 机 器 人 的 运动 时 ,根据 机 器 人 作业 任务 的 不 同 ,往往 需 要 知道 其 末 
端 执 行 器 的 位 置 和 姿态 。 我 们 把 刚体 相对 于 空间 某 一 坐标 系 的 位 置 和 姿态 , 称 为 刚体 的 位 姿 。 

我 们 采用 这 样 的 方法 来 描述 刚体 的 位 置 和 姿态 : 首先 规定 一 个 参考 坐标 系 ,相对 于 该 坐 
标 系 ,点 的 位 置 可 以 用 一 个 3X1 的 位 置 矢 量 表示 ; 刚体 的 姿态 可 用 3X3 的 旋转 矩阵 来 表示 。 
而 刚体 位 置 和 姿态 (位 姿 ) 的 统一 描述 通过 一 个 4 义 4 齐 次 变换 矩阵 表示 。 描 述 刚体 位 姿 的 方 
法 有 齐 次 变换 法 .四 元 数 法 . 旋 量 法 .矢量 法 等 ,本 章 重点 介绍 齐 次 变换 法 。 


1.1 刚体 的 位 姿 描述 


1.1.1 位 置 描述 
首先 建立 一 个 直角 坐标 系 {A} ,如 图 1.1 所 示 , 则 空间 中 任 一 点 P 的 位 置 可 以 用 3X1 的 
列 矢量 ,即位 置 拓 量 ^P 表示 为 : 


小 
Ap= |p, CI 

p 0 
其 中 ,p,、p, 和 p. 是 点 P 在 坐标 系 {A) 中 的 3 个 坐标 分 量 ,^P 的 
上 标 A 代表 选 定 的 参考 坐标 系 {A} ,4P 称 为 位 置 矢量 。 除 了 直角 2 2 
反 标 和 之 外 ,我 们 也 可 上 用 球 华 标 系 上 者 团 村 华 标 系 料 述 间 中 
点 的 位 置 。 XA 

2 姿态 描述 图 1.1 位 置 描述 


为 了 研究 机 器 人 在 空间 的 运动 状况 ,不 仅 要 确定 物体 的 空间 位 置 ,而 且 需 要 确定 物体 的 空 
间 姿 态 (也 称 为 方位 ) 。 物 体 姿 态 的 描述 可 以 采用 欧 拉 角 旋转 矩阵 .RPY 角 等 方法 。 以 下 介 
绍 使 用 旋转 矩阵 描述 物体 姿态 的 方法 。 

通常 采用 与 刚体 固 联 的 坐标 系 来 描述 其 在 直角 坐标 系 下 的 姿态 。 对 于 刚体 ,我 们 可 以 建 
立 与 之 固 联 的 一 直角 坐标 系 {B} ,然后 用 与 坐标 系 {B} 的 3 个 坐标 轴 上 的 单位 矢量 zs、yp 和 
zp( 当 用 坐标 系 {A} 的 坐标 表达 时 ,这 3 个 单位 矢量 被 写成 *s、yp 和 zp ) 相 对 于 参考 坐标 
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系 {A}) 的 方向 余弦 组 成 的 3X3 的 矩阵 表示 刚体 B 相 
对 于 坐标 系 {A}) 的 姿态 ,如 图 1. 2 所 示 。 
al i a 
oR =[ zp SY Ll" 入 汪 (1.2) 
an i 
其 中 ,3R 表示 刚体 B 在 坐标 系 {A} 中 姿态 的 旋转 矩阵 。 
旋转 矩阵 SR 的 9 个 元 素 中 ,只 有 3 个 是 独立 的 。 
由 于 3R 的 3 个 列 向 量 ^zp、 ya 和 ^zp 都 是 单位 矢 
量 , 且 两 两 相互 垂直 , 即 


A A 2 A 0 二 
TpB°* B= 名 : 时 二 法 放 : 寺 i %B"” BB 


ey 图 1.2 刚体 的 姿态 描述 
因此 ,矩阵 SR 是 一 个 正 交 和 矩阵 , 即 
帮主 (1.3) 
后 面 我 们 经 常用 到 的 旋转 变换 矩阵 是 绕 xz 轴 、 绕 y 轴 或 者 绕 z 轴 旋 转 某 一 角度 9, 则 旋转 
矩阵 分 别 为 : 


0 0 由 0 
R,.(0)= |I0 cos0 —sin0|=|0 c0 一 530 (1.4) 
0 Sin0 cos0 “1 
cos0g 0 sin0 co 0 wo 
wo-| 区 -| 人 (1.5) 
一 Sin0 0 cosO S04. Qs GO 
cosO 一 Sin0 0 CO 一 站 i 
R.(0)= |sin0 cosg 0|=|ls% cc 0 (1.6) 
0 0 了 -| 


式 (1.4) 一 式 (1.6) 中 ,cb 一 cos0,s0 一 sin0 。 

综 上 所 述 ,我 们 可 以 采用 位 置 矢量 描述 点 的 位 置 , 用 旋转 矩阵 描述 物体 的 姿态 。 

1.1.3 坐标 系 的 描述 

为 了 完全 描述 刚体 B 在 空间 中 的 位 姿 ,通常 将 刚体 也 与 坐标 系 {B) 固 联 , 坐 标 系 {B} 的 原 
点 一 般 选 在 物体 的 特征 点 上 ,如 质心 或 者 对 称 中 心 等 。 相 对 于 参考 坐标 系 {A), 坐 标 系 {B}) 原 
点 的 位 置 用 位 置 矢量 ^Ppore 描述 ,而 旋转 矩阵 8R 用 来 描述 坐标 系 {B) 的 姿态 (方位 )。 因 此 ， 
可 以 用 旋转 矩阵 4R 和 位 置 撩 量 ^Pporc 描述 坐标 系 {B}, 则 刚体 B 在 参考 坐标 系 {A } 中 的 位 
姿 可 利用 坐标 系 {B) 进 行 描述 。 即 

{B} = {8R, PaoRc ) : (1.7) 

可 见 , 坐 标 系 的 描述 概括 了 刚体 位 置 和 姿态 的 描述 。 当 表示 位 置 时 ,上 式 中 的 旋转 矩阵 8R 为 
单位 矩阵 ; 当 表 示 姿 态 时 ,位 置 矢量 ^Paonc 一 0。 


1.2 坐标 变换 


空间 中 任意 点 已 从 一 个 坐标 系 描述 到 另 一 个 坐标 系 描述 之 间 的 映射 关系 可 以 用 坐标 变 
换 来 曾 述 。 在 研究 机 器 人 的 运动 学 时 ,常用 的 坐标 变换 有 平移 变换 .旋转 变换 和 一 般 坐 标 
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1.2.1 平移 坐标 变换 

平移 坐标 变换 如 图 1. 3 所 示 ,参考 坐标 系 {A} 和 坐标 系 {B) 姿 态 相 同 ,但 两 坐标 系 的 原点 
不 重合 ,坐标 系 {B} 是 坐标 系 {A ) 经 过 平移 得 到 的 。 设 空间 中 任意 一 点 卫 , 它 相对 于 坐标 系 
{A} 的 坐标 用 位 置 矢量 ^P 表示 ; 同 理 ,点 P 在 坐标 系 {B) 中 的 坐标 可 用 位 置 矢量 ?P 表示 。 
坐标 系 {A ) 平 移 到 坐标 系 {B} 的 距离 用 ^Pporc 表示 。 由 于 矢量 ^P 和 2?P 具有 相同 的 姿态 , 那 
么 点 P 相对 于 坐标 系 {A } 的 位 置 矢量 ^P 可 由 矢量 相 加 得 出 , 即 

AP = BP + Ppoge (1. 8) 

式 (1. 8) 称 为 坐标 平移 ,或 者 平移 映射 。 需 要 注意 的 是 ,只 有 当 坐 标 系 {A} 和 坐标 系 {B} 

的 姿态 相同 时 , 式 (1. 8) 才 适用 。 


图 1.3 平移 坐标 变换 


1.2.2 旋转 坐标 变换 

旋转 坐标 变换 如 图 1.4 所 示 ,坐标 系 {B} 和 参考 坐标 系 {A } 有 共同 的 坐标 原点 ,但 姿态 不 
同 。 坐 标 系 {B} 是 坐标 系 {A} 经 过 旋转 (旋转 矩阵 为 6AR) 得 到 的 。 同 一 个 点 P 在 坐标 系 {A)} 和 
{B) 中 的 坐标 分 别 表示 为 ^P 和 ?P, 则 矢量 ^*P 和 3P 有 如 下 变换 关系 : 

4P =4R3P (1.9) 

式 (1. 9) 称 为 坐标 旋转 ,或 者 旋转 映射 。 

例 1.1 坐标 系 {B} 相 对 于 坐标 系 {A) 绕 zx 轴 旋 转 30" ,如 图 1.5 所 示 , 这 里 zx 轴 指 向 为 由 
纸 面向 外 。 假 设 点 P 在 坐标 系 {B) 中 的 位 置 撩 量 为 *P 二 [0.0 2.0 0.0]'。 求 点 P 在 坐标 系 
{A} 中 的 位 置 矢量 。 


2B 24 


x 
4 XB O 7X4 


图 1.4 旋转 坐标 变换 图 1.5 坐标 系 {A) 绕 zx 轴 旋 转 30" 得 到 坐标 系 {B) 
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解 : 在 坐标 系 {A) 中 写 出 坐标 系 {B) 的 单位 矢量 ,并 将 它们 按 列 组 成 旋转 矩阵 ,得 到 
0.866 一 0.500 0.000 
nm 0. 866 ooo 
0.000 0.000 1.000 
由 式 (1.9) 可 得 


0.866 一 0.500 0.000] [0.0 =:1,000 
Ap=sR*P=|0.500 0.866 “0.000| |2.0|= | 1.732 
0.000 ”0.000 1.000| |0.0 0. 000 


1.2.3 一 般 坐 标 变换 

考虑 到 坐标 变换 的 一 般 情况 ,通常 坐标 系 {B} 和 参考 坐标 系 {A} 没 有 共同 的 坐标 原点 , 即 
有 一 个 矢量 偏 移 ; 且 两 坐标 系 的 姿态 也 不 相同 。 坐 标 系 {B} 的 坐标 原点 相对 于 {A ) 坐 标 系 的 
位 置 用 矢量 ^Pporc 表示 ,同时 坐标 系 {B} 相 对 于 坐标 系 {A ) 的 旋转 用 AR 描述 , 任 一 点 P 在 坐 
标 系 {A} 和 {B) 中 的 描述 分 别 ^4P 和 3P 表示 ,如 图 1.6(a) 所 示 。 

对 于 上 述 情况 可 以 这 样 处 理 : 先 将 3P 变换 到 一 个 中 间 坐 标 系 {C}) ,坐标 系 {C} 和 (A) 的 
姿态 相同 , 且 和 坐标 系 {B } 的 原点 重合 ,如 图 1.6(b) 所 示 。 坐 标 系 {A}、{B) 和 {C) 关 系 为 : 坐 
标 系 {B ) 则 由 {C } 旋 转 (旋转 矩阵 为 5R) 得 到 ,坐标 系 {C} 由 {A} 平 移 矢 量 ^APaonc 得 到 , 即 

<P 一 5R2P 一 人 R2P 

p="P 民居 de 
整理 上 式 可 得 

Ap'= RP 和 Po CI L608 
由 式 (1.10) 可 以 看 出 ,一 般 情况 下 的 坐标 变换 包括 了 旋转 坐标 变换 和 平移 坐标 变换 。 


hc 
| 


X4 


(a) 情况 一 人 ) 情况 二 


图 1.6 一 般 情况 下 的 坐标 变换 


1.3 齐 次 变换 

1.3.1 齐 次 变换 矩阵 

齐 次 坐标 表示 法 是 以 N 十 1 维 矢量 表达 N 维 矢 量 的 方法 。 齐 次 坐标 提供 了 用 和 矩阵 运算 
把 二 维 、 三 维 甚至 高 维 空间 中 的 一 个 点 集 从 一 个 坐标 系 变换 到 另 一 个 坐标 系 的 有 效 方法 。 

一 般 情况 下 的 坐标 变换 式 (1. 10) 描 述 了 在 不 同 坐标 系 中 的 位 姿 AP 和 3P 之 间 的 关系 ,由 
于 变换 式 对 于 点 3P 来 说 是 非 齐 次 的 ,我 们 可 以 将 这 个 等 式 写成 等 价 的 齐 次 变换 形式 。 因 为 三 
维 矩 阵 计 算 不 能 充分 描述 齐 次 变换 ,所 以 增加 了 矩阵 的 维 数 ,将 等 式 改写 为 四 维 矩 阵 的 形 
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A 人 
| "| cl BORG e C1.11) 
1 0 1 1 


a CL 
式 (1.12) 中 4X4 的 矩阵 4T 称 为 齐 次 变换 矩阵 , 即 : 


式 , 即 : 


写成 矩阵 形式 为 : 


人 A 
网 了 和 让 和 和 
A > | 3 
1X3 
注意 , 式 (1. 12) 中 4P 和 *P 均 为 4X1 的 位 置 矢量 , 即 ， 
和 六 “ 
A B 
Ap = Rx Bp = 3 
p: » 
1 1 


实际 上 , 齐 次 变换 矩阵 4T 描述 了 坐标 系 {B} 相 对 于 坐标 系 {A }) 的 位 置 和 姿态 ,3$T 的 第 4 
列 和 撩 量 ^Pporc 描述 了 {B} 的 坐标 原点 相对 于 {A ) 的 位 置 , 其 他 3 个 列 矢量 分 别 表 示 {B} 的 3 
个 坐标 轴 相对 于 {A) 的 方向 。 

例 1.2 坐标 系 {B} 如 图 1.7 所 示 , 它 先 绕 坐 标 系 {A)》 
的 xs。 轴 旋 转 了 30°, 接 着 沿 {A) 的 zs 轴 平 移 10 个 单位 ,最 
后 沿 {A} 的 ya 轴 平 移 5 个 单位 。 已 知 SP=[3 7 0 1]'， 
求 ^P。 

解 : 由 题 可 知 ,旋转 矩阵 8R 一 R,(30") ,平移 矢量 为 
^Paope=[10 5 0] ; 则 


0.866 一 0.5 0 10]. 
全 = | ls 0.5 0.866 0 5 
3 0 1 0 0 1 区 
0 0 g 1 图 1.7 经 平移 和 旋转 的 坐标 系 ({B} 
由 ?PP 一 [3 7 0 1]7, 可 得 
0.866. 一 0.5 0 1o] 9. 098 
PR 0.866 0 5||7| |12.562 
0 0 i ‘ol 0 
0 0 0 浏 记 


WAP= 19. 0908» 12. .5862 -QI 
例 1. 2 中 的 齐 次 变换 矩阵 4T 描述 了 坐标 系 {B} 相 对 于 坐标 系 {A) 的 位 置 和 姿态 ,可 解释 
如 下 : 
{B} 的 坐标 原点 相对 于 {A }) 的 位 置 为 : [0 5 0 1]，; 
{B}) 的 三 个 坐标 轴 相 对 于 {A ) 的 方向 分 别 是 : [0.866 0.5 0 0]'、[ 一 0.5 0.866 0 
OT 0 1 Ol 
1.3.2 齐 次 变换 矩阵 的 逆 


给 定 坐 标 系 {A} 和 {B}, 已 知 {B} 相 对 于 {A} 的 描述 为 8T, 求 {A} 相 对 于 {B}) 的 描述 
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为 
对 于 旋转 矩阵 4R ,根据 正 交 和 矩 阵 的 性 质 可 以 得 到 : 


RAR 2 {1 
根据 闭 矩 阵 的 定义 ,可 以 得 到 如 下 关系 : 
TST =1 Cl: 
即 
六 ed 用 | a | (1 
01>3 | 0xs 1 0xs 1 | 
将 式 (1. 16) 左 边 的 第 一 个 矩阵 的 第 一 行 和 第 二 个 矩阵 的 第 二 列 相 乘 ,其 结果 与 式 (1. 
右边 的 对 应 项 相等 ,可 得 
ARY + “Pporc 二 0 ch 
进一步 得 到 
Y= =SR "Ryoge ey (1 


因此 ,对 于 转换 矩阵 8T ,其 道 矩 阵 不 等 于 原 矩 阵 。 经 过 上 面 的 推导 ,可 得 ; 


Ar 一 ! B SR 0 
BT 二 AT= Cl: 
01x3 1 
例 1.3 坐标 系 {B} 如 图 1.8 所 示 , 先 绕 坐 标 系 yo {B} 


位 ,最 后 沿 {A} 的 yA 轴 平 移 3 个 单位 得 到 ST。 求 4T。 


Yh {4} 
解 : 由 题 可 知 ,旋转 和 矩阵 R= 二 R.(30") ,平移 矢量 
0 .tl] 
0.866 一 0.5 0 4 2 4 
8R <Ppoge| 10.5 0. 866 0 3 图 1.8 经 坐标 系 {A) 平 移 和 旋转 后 
| 1 - 0 0 T 得 到 的 坐标 系 {B) 
0 0 0 
根据 式 (1. 19) 可 得 
0.866 0.5 0 4.964 
条 一 8R “PaoRc | 一 0.5 0.866 0 一 0.598 
[山寺 1 0 0 i 0., 
0 0 co 


1.4 运动 算 子 : 平移 旋转 和 变换 
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齐 次 变换 8T 既 可 以 表示 同一 点 在 两 坐标 系 {A} 和 {B) 中 描述 的 映射 ,也 可 以 描述 坐标 系 
{4B} 相对 于 (A) 的 位 姿 。 另 外 , 齐 次 变换 还 可 以 用 来 作为 运动 算 子 。 算 子 是 用 于 坐标 系 间 点 


的 映射 的 通用 数学 表达 式 , 包 括 点 的 平移 算 子 .旋转 算 子 等 。 
1.4.1 平移 算 子 


平移 ,即将 空间 中 的 一 个 点 沿 着 一 个 已 知 的 矢量 方向 移动 一 定 距离 。 这 里 需要 注意 的 是 ， 
对 空间 中 一 点 实际 平移 的 描述 仅 与 一 个 坐标 系 有 关 。 如 图 1. 9 所 示 ,在 坐标 系 {A} 中 ,点 己 由 


初始 位 置 ^P, 经 过 平移 或 旋转 到 达 位 置 ^P, ,移动 矢 量 用 4O 来 表示 。 
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如 下 : 


式 申 


图 1.9 平移 算 子 
平移 前 后 位 置 撩 量 ^P; 和 ^P, 之 间 的 关系 如 下 : 
^P， 一 ^P; 十 ^0 C1. 20) 
将 上 述 关 系 写 成 算 子 的 形式 有 : 
4P,， 一 Trans(40O)^P， C1.21) 
移动 矢量 ^O0 来 表示 平移 的 大 小 和 方向 。 符 号 Trans () 表 示 平 移 算 子 ,其 特殊 的 表示 形式 
L205 
Trans(*0) = ey (1. 22) 
0 昌平 闪 
OR 
2 Has 是 平移 矢量 40 的 分 量 。 因 此 ,可 以 将 Trans(^0) 写 成 如 下 形式 : 
T ;x3 ^0 
Trans(4 二 J 22 
(20) 人 本 2555 
1.4.2 旋转 算 子 


旋转 算 子 是 旋转 矩阵 的 另 一 种 表示 形式 , 它 是 将 一 个 矢量 A， 用 旋转 变换 成 一 个 新 的 矢 


量 ^P，, 。 同 平移 一 样 , 对 空间 中 一 点 实际 旋转 的 描述 也 仅 与 一 个 坐标 系 有 关 。 在 坐标 系 {A ) 
中 , 某 点 已 旋转 前 的 位 置 用 矢量 ^P; 表示 ,旋转 后 的 位 置 用 ^P, 表示 , 则 ^P， 和 ^P, 的 关系 有 


两 种 


式 中 
标 系 {A 


旋转 


1. 用 旋转 矩阵 R 表示 
将 R 作为 旋转 算 子 作用 于 矢量 ^P, ,得 到 新 的 矢量 ^P, ,表示 如 下 : 
Ap, 一 及 4Pi Cl. 24) 
,旋转 矩阵 R 作为 算 子 时 ,没有 上 标 或 者 下 标 ,因为 矢量 *P， 和 “^P， 都 是 相对 于 同一 个 坐 
) ,不 涉及 第 二 个 坐标 系 。 

站 全 欠 变 换 Rk (0) 表 示 1 
et 胞 和 “^P， 的 关系 表示 如 下 : 

Ap, —Rix COP (1. 25) 
旋转 算 子 Rk (0) 可 以 明确 表示 出 绕 哪 个 轴 旋 转 以 及 旋转 的 角度 , 即 Rk (90) 表示 绕 KK 轴 
9 角度 。 如 将 绕 Z 轴 旋转 0 角 的 算 子 写成 齐 次 变换 和 矩阵 ,其 中 位 置 矢量 的 分 量 为 0, 表示 


如 下 : 


10 硬 | 机 器 人 控制 一 一 运动 学 、 控 制 器 设计 、 人 机 交互 与 应 用 实例 


cosO 一 sin0 0 0 c0 一 吧 0 0 
Si ‘a0 QO 0 50 :的 Q 0 
Rk (90)=R,.(0)= = Il 
0 0 lL.0 0 0 i 
0 0 (py 0 0 QL 
例 1.4 矢量 *Pj==[0 2 0 1]', 如 图 1.10 所 示 。 计 算 绕 > 轴 旋 转 30" 得 到 的 新 


矢量 ^P，。 
解 : 将 矢量 “P; 绕 z 轴 旋转 30" 得 到 的 旋转 矩阵 与 某 一 坐标 系 绕 参 考 坐 标 系 的 = 轴 旋 转 
30 得 到 的 旋转 矩阵 相同 。 所 以 旋转 算 子 为 : 


0.866 一 0.5 局 三. 局 
0.5 OR66 CG 0 
R.(30°) = 
0 0 i 
0 0 Gus 
由 题 意 可 知 ^P,=[0 2 0 1]', 则 新 矢量 ^P, 为 : 
0.866 一 0.5 人 有 | I0 一 】 
A Ew :$3 Q. 866” 和 人工 你 | -| 区 国人 3 
和 二 0995 三 
0 < co 0 0 
0 0 人 全 1 | 


1.4.3 变换 算 子 

位 置 矢量 描述 的 是 平移 前 、 后 的 位 置 关系 ,旋转 矩阵 可 以 描述 旋转 前 、 后 的 位 置 关系 ,而 变 
换算 子 则 同时 描述 了 点 在 某 一 坐标 系 内 平移 和 旋转 的 情况 。 变 换算 子 是 坐标 变换 的 另 一 种 表 
示 形 式 , 它 是 将 一 个 矢量 ^P, 在 某 一 坐标 系 {A) 中 ,经 算 子 TT 平移 并 旋转 得 到 一 个 新 的 矢 
量 4P,,^P| 和 4^P, 的 关系 如 下 : 

apy =TAp, (1. 27) 
这 里 需要 注意 的 是 ,虽然 式 (1. 27) 和 式 (1. 12) 意 义 不 同 ,但 数学 
描述 是 相同 的 。 同 平移 和 旋转 算 子 一 样 ,变换 算 子 的 描述 也 只 涉 
及 一 个 坐标 系 ,所 以 算 子 T 没有 上 下 标 。 

例 1.5 已 知 矢量 ^4P,=[3 7 0 1]7, 如 图 1.10 所 示 , 将 
其 绕 z。 轴 旋转 30", 然 沿 着 zxA 轴 平移 10 个 单位 ,最 后 沿 y。 轴 
平移 5 个 单位 得 到 新 的 矢量 ^P，, 求 ^P，。 

解 : 题 中 描述 了 矢量 ^P, 在 坐标 系 {A } 中 平移 和 旋转 的 情 
况 , 因 此 可 实现 上 述 平移 和 旋转 的 变换 算 子 了 为 : 

0.866 一 0.5 0 .10 


0.8 0..8606 TGS 5 
各 三 图 1.10 矢量 ^P, 经 旋转 和 


A 平移 得 到 ^P 
0 0 0 1 人 


题 中 已 知 4Pj==[3 -7 0 1]J', 则 由 式 (1. 27) 可 得 ^P, 为 : 


0 B66 —d 6 Qo Ol | 9. 098 
0.5 O866050 dl 7 12: 562 
Pp; =T Pi 一 
0 0 1 0 0 
0 0 et , 沽 1 
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1.5 姿态 的 其 他 描述 方法 


前 面 介绍 了 采用 3X3 的 旋转 矩阵 描述 物体 的 姿态 ,本 节 将 介绍 姿态 的 其 他 描述 方法 。 由 
于 旋转 矩阵 既 可 以 看 成 是 映射 ,又 可 看 成 算 子 ,还 可 以 作为 物体 姿态 的 描述 ,用 途 不 同时 ,表示 
方法 自然 也 就 不 同 。 旋 转 矩 阵 作为 算 子 或 映射 使 用 时 ,起 到 运算 的 作用 。 根 据 和 矩阵 的 运算 规 
则 ,使 用 十 分 方便 ,但 用 旋转 答 阵 确定 姿态 时 ,并 不 方便 。 旋 转 抢 阵 的 9 个 元 素 中 ,只 有 3 个 是 
独立 的 ,但 是 我 们 需要 输入 它 的 9 个 元 素 ,如 果 我 们 能 用 3 个 参数 描述 会 显得 较为 简便 。 下 面 
介绍 的 这 些 表 示 方 法 只 需要 3 个 或 者 4 个 参数 。 

1.5.1 X-Y-Z 固定 角 坐 标 系 

X-Y-Z 固定 角 坐 标 系 姿态 的 表示 方法 为 : 先 将 坐标 系 {B) 的 初始 姿态 和 固定 参考 坐标 系 
{A} 重 合 ,然后 将 坐标 系 {B} 绕 {A} 的 XX。 轴 旋 转 7 角 , 再 绕 着 Y。 轴 旋 转 8 角 , 最 后 绕 Z。 轴 
旋转 a 角 。 我 们 称 对 应 于 固定 参考 坐标 系 X.Y 和 2 轴 旋转 的 角 分 别 为 回转 角 、 俯 仰角 和 偏转 
角 。 这 里 的 3 次 旋转 都 是 绕 着 固定 参考 坐标 系 的 轴 进 行 的 ,如 图 1. 11 所 示 。 


0 


图 1.11. X-Y-Z 固定 角 坐 标 系 (分 别 绕 固定 参考 坐标 系 的 X、Y 和 2Z 轴 旋 转 ) 


可 以 通过 3 个 旋转 矩阵 的 相 乘 推导 出 等 价 旋转 矩阵, 即 : 
ARxyz (7,B,a) =Ry (a)Ry (BRx (7) 
ho 


cB 0 spB 
SQ CQ 0 0 4 ， 洛 0 ney = 


0 0 Wp Ba ww 
式 中 ,ca 是 余弦 函数 cosa 的 简写 ,sa 是 正弦 函数 sina 的 简写 ,cB、sB.cY 和 7y 分 别 是 它们 对 应 
正 余弦 函数 的 简写 。 而 表达 式 中 旋转 矩阵 相 乘 的 顺序 ,是 按照 “从 右 向 左 ? 的 原则 ,依次 右 乘 基 
本 旋转 矩阵 ,将 式 (1. 28) 中 的 矩阵 相 乘 可 得 

cacB casBsY— sacy casBcy + sasy 
ARxyz (YBya) = |sacB sasBsy 十 cacy sasBcy — casy 
8 cBsy | cBeY 
需要 注意 的 是 ,上 式 只 在 旋转 顺序 按照 先 绕 X 4 轴 旋转 y 角 , 再 绕 着 YA 轴 旋 转 B 角 , 最 后 绕 
Za 轴 旋 转 a 角 时 成 立 。 


1.5.2 Z-Y-X 欧 拉 角 坐标 系 

Z-Y-X 欧 拉 角 坐标 系 姿态 的 描述 方法 为 : 先 将 坐标 系 {B} 的 初始 姿态 和 固定 参考 坐标 系 
{A) 重 合 , 然 后 将 坐标 系 {B)} 绕 着 自身 的 Zs 轴 旋 转 a 角 , 再 绕 着 Ys 轴 旋 转 B 角 , 最 后 绕 Xs 
轴 旋 转 y 角 。 我 们 称 这 样 三 个 一 组 的 旋转 为 欧 拉 角 , 由 于 三 次 旋转 分 别 是 绕 着 运动 坐标 系 
{B}) 的 ZY 和 XX 轴 , 因 此 称 这 种 表示 姿态 的 方法 为 Z-Y-X 欧 拉 角 。 这 里 与 X-Y-2 固定 角 坐 
标 系 姿态 表示 法 最 大 的 不 同 是 ,三 次 旋转 都 是 绕 运 动 坐标 系 {B} 的 各 轴 旋 转 , 而 不 青 是 绕 固定 


a 83% 兴 


《二 


(1 29) 


12 大 | 机 器 人 控制 一 一 运动 学 、 控 制 器 设计 、 人 机 交互 与 应 用 实例 


参考 坐标 系 {A } 的 各 轴 进 行 旋转 ,如 图 1. 12 所 示 。 


有 到 


Xa 
Xp 


图 1.12 2Z-Y-X 固定 角 坐 标 系 (分 别 绕 运动 坐标 系 的 Z、Y 和 X 轴 旋 转 ) 


坐标 系 {B } 绕 着 自身 的 Zs 轴 旋 转 a 角 后 ,将 X 旋转 到 X'、Y 旋转 到 YY 位 置 ,依次 往 后 ， 
每 经 过 一 次 旋转 ,都 在 旋转 后 得 到 的 轴 上 多 加 一 个 “ 撤 号 ”, 如 图 1. 12 所 示 。 由 Z-Y-X 欧 拉 
角 旋 转 后 得 到 的 等 价 矩 阵 用 3Rzywx (7Y,B,a) 表 示 , 这 里 下 标 上 的 “ 撤 号 ”表示 是 由 欧 拉 角 描述 
的 旋转 。 
因为 三 次 旋转 都 是 相对 运动 坐标 系 进行 的 ,根据 “从 左 至 右 ”的 原则 ,依次 左 乘 每 次 旋转 对 
应 的 矩阵 ,可 得 经 过 一 系列 旋转 后 的 等 价 旋转 矩阵 : 
ARyyx' (yy8ya) 一 Rz(a)Rv(CB)Rx(C7) 


eh 0 HH 0 
0 0 | (0 


= Sy” “ey 


Eo 
5 0 
0 0 

将 上 式 右边 的 矩阵 相 乘 , 从 而 可 以 得 出 : 

cacB casBsy — sacy we 


ARyyx' (YBsa) = |sacB sasBsY 十 cacy sasBcY — casy Cl 
一 cps7 cpBcy 

比较 式 (1. 29) 和 式 (1. 31) 可 以 发 现 ,采用 Z-Y-X 欧 拉 角 坐 标 系 和 X-Y-Z 固定 角 坐 标 系 
描述 姿态 得 到 的 结果 完全 相同 , 即 3 次 都 绕 固定 坐标 系 的 轴 旋 转 的 最 终 姿 态 与 以 相反 顺序 三 
次 都 绕 运动 坐标 系 的 轴 旋 转 的 最 终 姿态 是 相同 的 。 因 此 ,也 可 以 用 式 (1. 31) 来 求解 同一 个 已 
知 旋转 矩阵 的 Z-Y-X 欧 拉 角 。 

例如 ,在 Z-Y-X 欧 拉 角 中 ,坐标 系 {B} 绕 Zs 旋转 30" ,再 绕 Yp 旋转 45” "最 后 绕 Zs 旋转 
60" ,要 求解 经 过 这 一 系列 旋转 变换 得 到 的 旋转 矩阵 。 


1.5.3 Z-Y-Z 欧 拉 角 坐标 系 

Z-Y-Z 欧 拉 角 坐标 系 表示 姿态 的 方法 为 : 先 将 坐标 系 {B} 的 初始 姿态 和 固定 参考 坐标 系 
{4A} 重合 ,然后 将 坐标 系 {B} 绕 着 自身 的 Zs 轴 旋 转 a 角 , 再 绕 着 Ys 轴 旋 转 B 角 , 最 后 绕 Zp 
轴 旋 转 Y 角 。 

与 Z-Y-X 欧 拉 角 坐 标 系 描述 姿态 的 方法 相同 ,Z-Y-Z- 欧 拉 角 坐标 系 表 示 姿 态 的 方法 也 是 
三 次 旋转 都 围绕 运动 坐标 系 {B} 进 行 ,又 因 三 次 旋转 的 顺序 依次 绕 Z 轴 、Y 轴 和 2Z 轴 , 因 此 称 
这 种 描述 为 Z-Y-Z 欧 拉 角 。 

同 Z-Y-X 欧 拉 角 坐 标 系 相同 ,由 于 三 次 旋转 都 是 相对 于 运动 坐标 系 而 言 的 ,根据 “从 左 
至 右 ” 的 原则 ,依次 左 乘 对 应 的 旋转 和 矩阵 ,可 得 最 终 的 等 价 旋 转 和 矩阵 为 

ARyyz (YsBya) =Ry(a)Ry (BPR; C7) 
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ea —s& 0 eB © Bh ley 二 9 
一 . ca ， 0 1 ， 2 (ap 4 ， 以 -32) 
0 0 1 l=sB: 0 部 15 0 1 
将 上 式 右边 的 矩阵 相 乘 ,从 而 可 以 得 出 : 
os 有 cy = sy 一 000857 一 5356C7 0 
SR (YsBra) = lsacBeY 十 casy —sacBsyYi+eacy sas (1. 33) 
—sBceY sBsy cB 


这 里 需要 补充 一 点 ,上 述 三 种 表示 姿态 的 方法 中 , 求 最 终 的 等 价 旋转 矩阵 时 ,矩阵 是 左 乘 
还 是 右 乘 取 决 于 旋转 是 相对 于 固定 坐标 系 还 是 运动 坐标 系 , 若 是 相对 于 固定 坐标 系 的 描述 , 则 
从 右 向 左 ; 反之 , 则 从 左 向 右 。 


1.5.4 等 效 轴 角 坐标 系 

前 面 介 绍 了 三 种 描述 姿态 的 常用 方法 : X-Y-2Z 固定 角 、Z-Y-X 欧 拉 角 和 2Z-Y-Z 欧 拉 角 ， 
这 三 种 方法 是 角 坐 标 系 表 示 法 中 三 种 典型 的 表示 方法 。 角 坐标 表示 法 的 原理 是 : 首先 将 坐标 
系 {B} 和 一 个 固定 的 参考 坐标 系 {A ) 重 合 , 然 后 每 次 将 {B) 绕 着 {A} 或 {B} 的 其 中 一 条 轴 旋 转 
一 定 的 角度 ,连续 旋转 三 次 。 角 坐标 表示 法 有 24 种 典型 的 表示 方法 ,其 中 12 种 为 固定 角 坐 标 
系 法 ,这 种 方法 每 次 旋转 都 只 围绕 着 固定 参考 坐标 系 {A )} 的 某 一 条 轴 ; 另外 12 种 称 为 欧 拉 角 
坐标 系 法 ,但 是 该 方法 每 次 旋转 都 是 绕 着 运动 坐标 系 {B} 的 某 一 条 轴 。 

下 面 介 绍 等 效 轴 角 坐标 系 表示 法 。 将 坐标 系 {B} 的 初始 姿态 和 固定 参考 坐标 系 {A) 重 
合 , 然 后 将 坐标 系 {B} 绕 矢量 ^K 按 右 手 定 则 旋转 0 角 。{B} 相 对 于 {A}) 的 姿态 用 %R。(9) 表 


示 , 也 称 作 等 效 轴 角 坐 标 系 法 。 矢 量 ^K 也 称 为 有 限 旋 转 的 等 效 轴 。 
等 效 轴 角 坐标 系 如 图 1. 13 所 示 , 当 旋转 轴 为 坐标 系 {A } 的 主轴 之 一 时 , 则 等 效 旋转 矩 


阵 为 : 

[1 0 0 

R,.(0)= |0 cosbO 一 Sin0 (1. 34) 
I0 sing -cos0 | 
| ews NO sind| 

R,(0) = 0 下 《1.35 
| 一 sin0 0 cos0| 
lcos0 一 Sing 0|] 

R,(0)= |sing cosg .0 人 
本 0 1 1.13 等 效 轴 角 坐标 系 

如 果 旋 转轴 为 一 般 轴 , 等 效 施 转 和 矩阵 如 下 

kev He Esksud— ks kkov0 TEs0 


Rr AY [Rok wo Et kek ude 
Ek kev —k, 0 k kv ks 


k,k.v0 —k,.s0 
k.k.v0 tc0 
式 中 ,vw9 二 1 一 cos0 ,矢量 KR=[k， > .J] ,9 的 符号 则 由 右手 定 则 确定 , 即 太 的 正方 向 为 
拇指 指向 。、 

例 1.6 坐标 系 {B) 的 初始 姿态 和 坐标 系 {A} 重 合 ,将 坐标 系 {B} 绕 矢量 ^ 太 一 [0. 707 
0.707 70.0]'(%KK 经 过 坐标 原点 ) 旋 转 30°, 求 坐标 系 {B} 的 描述 。 

解 : 将 0 二 30" 代 入 式 (1. 37) 中 ,可 得 坐标 系 描述 的 旋转 矩阵 分 量 , 如 下 : 


(1..37) 
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| 六 于 6 kk vO —ks0 k kw0t ks0 
Rig(0)= | 有 ROOT 十 ES kk kk.w0— ks0 
[Rk.v0 一 SO Ek ka0+ks0 kk.v0+ed 
| D033 VO67 0.354 

一 0. 06867983 一 Do354 

一 05354 ‘03654 0. 866 

由 于 旋转 轴 经 过 原点 , 即 原点 没有 变 , 所 以 位 置 矢量 为 [0 0 0]  , 则 : 
0.933 0.067 0.354 10 


A | 0.067 0.933 —0.354 5 
2 = 354 O34 -0.88 0 
0 0 0 0 


图 1.14 矢量 ^P, 经 旋转 和 平移 得 到 ^P， 


1.6 本 章 小 结 


本 章 首先 介绍 了 刚体 位 置 和 姿态 的 描述 方法 ,包括 用 位 置 矢量 描述 空间 中 点 的 位 置 ,采用 
旋转 矩阵 描述 物体 的 姿态 ,而 坐标 系 的 描述 则 概括 了 刚体 位 置 和 姿态 的 描述 。 然 后 讨论 了 空 
间 中 任意 一 点 从 一 个 坐标 系 到 另 一 个 坐标 系 的 描述 ,以 及 常用 的 坐标 变换 方法 ,如 平移 变换 、 
旋转 变换 和 一 般 坐标 变换 。 在 此 基础 上 ,进一步 研究 齐 次 坐标 变换 ,包括 齐 次 变换 和 矩阵 及 其 逆 
矩阵 。 由 于 齐 次 变换 还 可 以 作为 运动 算 子 ,因此 本 章 介 绍 了 以 齐 次 变换 为 基础 的 运动 算 子 ,有 - 
平移 算 子 .旋转 算 子 以 及 变换 算 子 。 最 后 ,介绍 了 除 旋转 矩阵 之 外 的 其 他 描述 姿态 的 方法 , 包 
括 X-Y-2 固定 角 坐 标 系 、.Z-Y-X 欧 拉 角 坐 标 系 等 姿态 的 表示 方法 。 这 些 内 容 作为 研究 机 器 
人 的 数学 基础 ,为 后 续 研 究 机 器 人 运动 学 .动力 学 以 及 控制 等 提供 理论 支撑 。 
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CHAPTER 2 


机 器 人 运动 学 是 研究 机 器 人 运动 的 几何 关系 ,不 考虑 产生 运动 的 力 和 力矩 ,只 研究 运动 的 
位 置 .速度 \. 加 速度 和 位 置 变量 对 其 他 变量 的 高 阶 导数 。 机 器 人 运动 学 是 研究 机 器 人 动力 学 、 
轨迹 规划 和 位 置 控 制 的 重要 基础 。 

机 器 人 运动 学 包括 两 方面 的 内 容 : 机 器 人 正 运 动 学 和 逆 运 动 学 。 给 定 机 器 人 各 个 关节 的 
角度 , 求 机 器 人 未 端 执行 器 的 位 置 和 姿态 ,这 就 是 正 运动 学 问题 ,也 称 机 器 人 运动 方程 的 表示 
问题 。 求 解 该 问题 比较 简单 ,而 且 它 的 解 是 唯一 确定 的 。 机 器 人 道 运 动 学 问题 也 称 机 器 人 运 
动 学 方程 的 逆 解 ,或 间接 位 置 求解 , 即 给 定 机 器 人 末端 执行 器 的 位 置 和 姿态 ,求解 可 到 达 给 定 
位 置 和 姿态 的 各 关节 的 角度 值 。 机 器 人 逆 运 动 学 问题 的 求解 比较 复杂 ,而 且 往往 有 多 个 解 。 
逆 运 动 学 问题 实际 上 是 一 个 非 线 性 超越 方程 组 的 求解 问题 ,其 中 包括 解 的 存在 性 .唯一 性 及 求 
解 的 方法 等 一 系列 复杂 问题 。 

机 器 人 的 运动 学 模型 包括 描述 机 器 人 各 连 杆 . 关 节 的 位 置 以 及 建立 在 各 关节 上 的 坐标 系 ， 
其 任务 之 一 就 是 确立 机 器 人 未 端 执行 器 的 位 姿 。 


2.1 连 杆 描述 和 关节 变量 


2.1.1 机 械 辟 的 构成 


机 器 人 本 体 一 般 是 一 台 机 械 臂 ,也 称 操作 臂 或 操作 手 , 可 以 在 确定 的 环境 中 执行 控制 系统 
指定 的 操作 。 其 臂 部 一 般 采用 空间 开 链 连 杆 机 构 , 其 中 的 运动 副 ( 转 动 副 或 移动 副 ) 常 称 为 关 
节 。 根 据 关节 配置 和 运动 坐标 形式 的 不 同 ,机 器 人 执行 机 构 可 分 为 直角 坐标 式 、 圆 柱 坐 标 式 、 
极 坐 标 式 和 关节 坐标 式 等 类 型 。 

如 图 2. 1 所 示 ,机 械 臂 可 以 看 成 是 由 一 系列 连 杆 通过 关节 依次 连接 而 成 的 开 式 运动 链 。 
第 一 个 固定 连 杆 连接 机 械 臂 的 基 座 (可 称 它 为 连 杆 0) ,第 一 个 可 动 连 杆 为 连 杆 1 ,第 二 个 可 动 
变 杆 为 连 杆 2…… 以 此 类 推 ,最 未 端的 连 杆 ”连接 着 机 械 辟 的 末端 执行 器 。 通 常 在 基 座 处 建 
立 一 个 固定 参考 坐标 系 , 称 为 基 坐 标 系 ; 在 末端 执行 器 建立 的 坐标 系 称 为 工具 坐标 系 ,一 般 用 
它 来 描述 机 械 臂 的 位 置 。 

从 本 质 上 看 ,关节 通常 可 分 为 移动 关节 和 转动 (旋转 ) 关 节 两 类 。 移 动 关 节 可 以 沿 着 基准 轴 
移动 ,而 旋转 关节 则 是 围绕 基准 轴 转 动 ,不 管 是 转动 还 是 移动 ,都 是 沿 着 或 者 围绕 着 一 个 轴 进 行 
的 ,这 被 称 为 一 个 运动 自由 度 。 每 一 个 转动 关节 提供 一 个 转动 自由 度 , 每 一 个 移动 关节 提供 一 个 
移动 自由 度 ,关节 个 数 通常 即 为 机 器 人 的 自由 度数 ,各 关节 间 是 以 固定 杆 件 相连 接 的 。 还 有 一 种 
特殊 的 关节 称 为 球 关 节 , 它 有 3 个 自由 度 ,一 个 球 关节 可 以 用 3 个 转动 关节 和 一 个 零 长 度 的 连 杆 
来 描述 。 为 了 确定 未 端 执行 器 在 三 维 空间 中 的 位 姿 , 机 械 臂 至 少 需要 6 个 关节 ,刚好 对 应 6 个 自 
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图 2.1 机 械 辟 的 构成 


由 度 , 其 中 3 个 用 来 确定 末端 执行 器 的 位 置 ,另外 3 个 则 用 来 确定 末端 执行 装置 的 方向 (姿势 ) 。 

2.1.2 连 杆 描述 

描述 连 杆 的 参数 有 两 个 ,无 论 形状 多 么 复杂 的 连 杆 ,实际 上 它 在 运动 时 能 提供 的 运动 学 功 
能 可 以 用 两 个 参数 来 确定 ,一 个 是 连 杆 长 度 a;_1, 另 ”yj 1 
一 个 是 连 杆 转角 a;_| ,如 图 2. 2 所 示 。 连 杆 坟 1 轴 ; 

如 图 2. 2 所 示 ,一 个 连 杆 两 端 连 接着 两 个 关节 ， 
为 了 描述 方便 ,我 们 规定 连 杆 i 左 端 的 关节 是 i, 因 
此 , 基 座 为 连 杆 0, 而 基 座 和 连 杆 1 之 间 的 关节 就 是 
关节 1, 事实 上 关节 1 就 是 机 器 人 的 第 一 个 关节 ; 算 
上 基 座 0, 一 个 n 自由 度 的 机 械 辟 就 有 nn 个 关节 和 
n 十 1 个 连 杆 。 对 于 图 中 的 连 杆 i 一 1, 其 左边 对 应 的 
是 关节 轴 i 一 1, 右 边 对 应 的 是 关节 轴 i, 两 个 关节 轴 
之 间 的 关系 便 由 这 个 连 杆 确定 。 在 三 维 空间 中 ,两 
个 关节 轴线 之 间 的 距离 和 角度 可 以 描述 它们 之 间 的 
位 置 关 系 。 图 2. 2 中 ,a,_| 就 是 关节 轴 i 一 1 和 关节 
轴 i 之 间 的 公 垂 线 , 即 为 连 杆 长 度 。 在 三 维 空间 中 总 是 可 以 找到 两 条 直线 之 间 的 公 垂 线 , 如 果 
它们 平行 , 则 有 无 数 条 公 垂 线 ; 如 果 它 们 不 在 一 个 平面 , 则 只 有 一 条 公 垂 线 ; 如 果 它 们 相交 , 则 
公 垂 线 就 是 一 个 点 。 总 之 ,关节 轴线 i 一 1 和 关节 轴线 i 之 间 的 距离 是 由 连 杆 i 一 1 来 确定 的 。 | 

除了 描述 两 条 轴线 之 间 的 距离 外 ,我 们 还 需要 描述 这 两 条 轴线 之 间 的 夹 角 , 即 连 杆 转角 。 
连 杆 转 角 是 关节 轴 i 一 1 绕 着 公 垂 线 a;_1 转动 到 和 关节 轴 i 平行 时 所 转 过 的 角度 a;_1 (按照 
右手 法 则 转动 , 公 垂 线 a,_| 方向 定义 为 从 轴 i 一 1 指向 轴 i)。 注 意 : 在 以 上 描述 中 , 连 杆 长 度 
a 和 连 杆 转角 a 的 下 标 都 是 i 一 1, 因 为 连 杆 也 是 第 i 一 1 根 连 杆 , 这 与 前 文 关于 连 杆 的 规定 是 
一 致 的 。 

通常 用 连 杆 长 度 a;_! 和 连 杆 转角 a;_| 来 描述 连 杆 i 一 1 本 身 的 特征 。 

2.1.3 连 杆 连接 的 描述 

1. 中 间 连 杆 连接 的 描述 

相 邻 的 两 个 连 杆 之 间 有 一 个 共同 的 关节 轴线 。 因 此 ,对 应 于 每 一 关节 轴线 i 都 有 两 条 公 
垂 线 与 它 垂 直 , 这 两 条 公 垂 线 之 间 的 距离 称 为 连 杆 偏 距 , 记 为 d;, 它 代表 连 杆 i 相对 于 连 杆 


图 2.2 连 杆 的 运动 参数 


第 2 章 ， 机 器 人 运动 学 建 模 | 加 17 


a 


i 一 1 的 偏 置 。 同 样 地 ,对 应 于 关节 轴线 i 的 两 条 公 垂 线 之 间 的 夹 角 称 为 关节 角 , 记 为 0; 。 

图 2. 3 表示 连 杆 i 和 连 杆 i 一 1 之 间 的 连接 关系 。 描 述 相 邻 连 杆 之 间 连 接 关系 的 参数 也 
有 两 个 。 第 一 个 参数 是 连 杆 偏 距 & ,用 来 描述 连 杆 i 和 连 杆 i 一 1 的 连接 关系 ; 另 一 个 参数 为 
关节 角 0; ,用 来 描述 连 杆 i 和 连 杆 i 一 1 之 间 的 连接 关系 。 注 意 : 参数 9; 和 4d, 都 有 正 负 号 之 
分 。 由 图 2. 3 可 知 ,对 应 于 关节 轴 红 的 两 条 公 垂 线 分 别 为 a;_! 和 a; ,它们 之 间 的 距离 为 d，， 
即 沿 着 关节 轴 :的 轴 向 ,ai-i 与 轴线 i 的 交点 到 a; 与 该 轴线 交点 之 间 的 距离 ,参数 4d; 反映 了 
两 个 连 杆 沿 着 轴 i 的 距离 。0,; 则 表示 绕 轴 i 将 a;_1 的 延长 线 转动 到 与 a; 平 行 时 所 转 过 的 角 
度 ,关节 角 反 映 了 两 个 连 杆 在 关节 轴 处 的 夹 角 。 通 过 连 杆 偏 距 和 关节 和 角 可 以 将 两 个 相 邻 连 杆 
之 间 的 相对 位 置 描述 清楚 。 


轴 六 1 轴 i 


图 2.3 描述 中 间 连 杆 连 接 的 参数 


2. 连 杆 参数 和 关节 变量 

因此 ,对 于 一 个 连 杆 ,需要 4 个 参数 对 其 进行 描述 ,其 中 两 个 参数 描述 连 杆 本 身 的 特性 , 另 
外 两 个 参数 描述 相 邻 连 杆 之 间 的 相对 位 置 。 连 杆 偏 距 d; 和 关节 角 0; 是 由 关节 设计 决定 的 ， 
反映 了 关节 的 运动 学 特性 。 如 果 关 节 i 是 一 个 转动 关节 ,那么 连 杆 i 一 1 和 连 杆 i 之 间 沿 着 关 
节 轴 线 ; 的 距离 d; 就 是 一 个 定 值 ,对 于 任意 给 定 的 机 器 人 ,该 值 不 会 发 生变 化 ,而 9; 则 会 改 
变 ,因此 0; 称 为 关节 变量 , 即 机 器 人 在 运动 过 程 中 它 会 发 生变 化 。 同 样 地 ,如 果 关 节 i 是 一 个 
移动 关节 ,那么 连 杆 i 一 1 和 连 杆 ; 之 间 的 夹 角 0; 就 是 一 个 定 值 ,变化 的 是 两 个 连 杆 沿 着 关节 
轴线 的 距离 d, ,此 时 d，, 称 为 关节 变量 。 


2.2 D-H 参数 法 建立 关节 坐标 系 


D-H 参数 全 称 为 Denavit-Hartenberg 参数 , 它 是 Denavit 和 Hartenberg 于 1955 年 提出 
的 。 后 来 有 学 者 利用 D-H 参数 对 机 器 人 进行 建 模 , 并 导出 其 运动 方程 。D-H 参数 方法 已 成 为 
表示 机 器 人 和 对 机 器 人 运动 进行 建 模 的 一 种 标准 方法 。 在 标准 的 D-H 参数 法 中 ,描述 机 械 臂 
中 的 每 一 个 连 杆 需要 4 个 运动 学 参数 ,分别 是 连 杆 长 度 a;_1 、 连 杆 转角 a;_1 、 连 杆 偏 距 4; 和 
关节 和 角 0; 。 本 节 将 介绍 使 用 D-H 参数 法 建立 机 器 人 关节 坐标 系 的 一 些 原则 。 

D-H 方法 是 为 每 个 关节 处 的 连 杆 坐标 系 建立 4X4 齐 次 变换 矩阵 ,来 表示 此 关节 处 的 连 
杆 与 前 一 个 连 杆 坐标 系 的 关系 ,并 通过 逐次 变换 ,最终 求 出 用 基 上 坐标 系 表示 的 末端 坐标 系 的 变 
换 矩 阵 。 为 了 确定 各 连 杆 之 间 的 相对 运动 和 位 姿 关 系 ,需要 在 每 个 连 杆 上 固 连 一 个 坐标 系 。 
固 连坐 标 系 的 命名 与 固 连坐 标 系 所 在 连 杆 的 编号 相关 。 连 杆 0 上 固 连 的 坐标 系 为 坐标 系 {0}， 
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因 连 杆 0 与 基 座 相连 ,固定 不 动 , 该 坐标 系 也 称 基 上 坐标 系 。 坐 标 系 {0} 通 常 作为 参考 坐标 系 描 
述 其 他 连 杆 坐标 系 的 位 置 和 方位 。 与 连 杆 1 固 连 的 坐标 系 称 为 坐标 系 {1} ,以 此 类 推 ,与 最 后 
一 个 连 杆 n 固 连 的 坐标 系 称 为 坐标 系 {n), 即 末端 坐标 系 。 

2.2.1 确定 和 建立 坐标 系 的 原则 

在 建立 坐标 系 时 应 按照 如 下 原则 确定 连 杆 i 的 坐标 系 {i)。 

(1) 坐标 系 xz 轴 : zx; 与 关节 轴 ; 重合 ,但 z 轴 的 指向 可 以 任意 设 定 。 

(2) 坐标 系 x 轴 : x; 与 连 杆 ; 的 公 垂 线 a, 重合 ,正方 向 为 由 关节 ;i 指向 关节 i 十 1。 

(3) 坐标 系 y 轴 : y; 按照 右手 法 则 确定 。 

(4) 坐标 原点 : 为 公 垂 线 a; 和 关节 轴线 i 的 交点 , 沿 着 公 垂 线 a, 由 关节 轴线 i 指向 关节 
轴线 i 一 1 是 x,; 的 正方 向 。 

连 杆 坐标 系 {i 一 1} 和 {i}) 的 表示 如 图 2.4 所 示 。 


轴 二 1 四; 
连 杆 并 1 


图 2.4 连 杆 坐标 系 


基于 以 上 原则 , 基 上 坐标 系 {0} 在 基 座 上 的 位 置 和 方向 可 以 任意 选择 ,只 要 z。 轴 沿 着 关节 轴 
1 的 方向 即 可 。 而 末端 坐标 系 {n) 只 需要 满足 x, 轴 与 z,_1 轴 垂 直 即 可 。 

通常 为 简化 起 见 , 基 坐 标 系 {0} 的 zx。 轴 除 了 需要 满足 其 正方 向 与 关节 轴 1 的 方向 相同 之 
外 ,假定 关节 变量 1 为 0 时 , 设 定 坐 标 系 {1} 和 {0} 重 合 , 即 a 二 0,ao 二 0; 当 关 节 1 为 转动 关节 
时 ,di 二 0; 当 关 节 1 为 移动 关节 时 ,0 二 0。 

同样 地 ,末端 坐标 系 {n}) 的 设 定 与 坐标 系 {0} 相 似 。 当 关节 为 转动 关节 时 ,选取 的 z， 
应 使 0, 一 0, 即 x, 轴 与 x,_1 轴 重 合 ,坐标 系 {n} 原 点 的 选取 应 使 d, 二 0。 当 关节 为 移动 . 
关节 时 ,选取 的 坐标 轴 zx, 应 满足 9, ==0 和 4d, 二 0, 而 坐标 原点 的 选取 应 使 zx, 轴 与 x,_1 轴 
重合 。 

需要 注意 的 是 ,根据 上 述 关 于 坐标 系 的 规定 ,建立 的 坐标 系 并 不 能 保证 其 唯一 性 。 例 如 ， 
z; 轴 虽 与 关节 轴 i 重合 ,但 轴 的 指向 却 有 两 种 可 能 。 此 外 ,对 于 移动 关节 ,在 选择 坐标 系 时 
也 有 一 定 的 任意 性 。 


2.2.2 D-H 参数 

对 于 每 个 关节 ,4 个 D-H 参数 (a,;_1、a;_1、d; 和 90;) 中 有 3 个 为 固定 常量 , 另 一 个 是 变 
量 。 对 于 转动 关节 ,0, 为 变量 ,其 他 3 个 为 常量 ; 对 于 移动 关节 , 连 杆 偏 距 4d, 为 变量 ,其 他 3 
个 参数 固定 不 变 。 这 种 描述 机 构 运 动 关系 的 规则 称 为 D-H 参数 方法 。 任 一 机 械 臂 各 连 杆 之 
间 的 运动 关系 都 可 以 通过 关节 变量 和 连 杆 参数 来 描述 。 根 据 上 述 关于 连 杆 坐标 系 的 规定 ,对 
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这 4 个 参数 的 定义 如 下 : 

(1) 连 杆 长 度 a; -1 : zi; 轴 方 向 上 >; 轴 和 >z;+1 轴 之 间 的 距离 。 

(2) 连 杆 转角 a;_1: x; 轴 方 向 上 z; 轴 到 xz,,1 轴 旋 转 的 角度 。 

(3) 连 杆 偏 距 a, : =; 轴 方 向 上 zxz,_1 轴 到 zx, 轴 之 间 的 距离 。 

(4) 关节 角 0; : zx; 轴 方向 上 zz 过 1 轴 到 xz, 轴 旋 转 的 角度 。 

若 给 出 上 述 4 个 参数 的 正 负 号 规则 , 则 可 以 完全 确定 机 械 臂 每 一 个 连 杆 的 位 姿 。 由 于 
a;-_1 表示 连 杆 的 长 度 ,通常 规定 c;,_; 二 0, 其 他 3 个 参数 则 可 正 可 负 。 

2.2.3 连 杆 坐标 系 建 立 的 步骤 

对 于 某 一 给 定 的 机 械 辟 ,建立 其 各 连 杆 坐标 系 的 步骤 如 下 。 

@ 找 出 各 个 关节 轴 并 画 出 其 轴线 。 

@ 找 出 并 画 出 相 邻 两 轴 i 和 i 十 1 的 公 垂 线 a, 或 两 轴线 的 交点 ,选取 两 轴线 的 交点 或 者 
公 垂 线 a; 与 关节 轴 i 的 交点 作为 坐标 系 {让 的 原点 。 

@ 规定 z; 轴 与 关节 轴 ; 重合 。 

@ 规定 z; 轴 沿 公 垂 线 a; 的 方向 由 关节 轴 i 指向 关节 轴 ;i1。 若 关节 轴 1 与 关节 轴 i 十 1 
相交 , 则 规定 zx; 轴 垂 直 于 关节 轴 ; 和 i 十 1 所 在 的 平面 。 

@) 按 右手 法 则 确定 y;。 

@ 当 第 一 个 关节 变量 为 0 时 ,使 坐标 系 {(1) 和 {0} 重 合 。 对 于 末端 坐标 系 {2) ,原点 和 z， 
的 方向 可 任 选 ,为 使 计算 简单 ,通常 在 选取 时 我 们 希望 坐标 系 {n} 的 连 杆 参数 尽 可 能 为 0。 

例 2.1 如 图 2.5 所 示 为 平面 三 连 杆 机 械 臂 ,其 中 3 个 关节 均 为 转动 关节 ,也 称 RRR( 或 
者 3R) 机 构 ,请 建立 该 机 构 的 连 杆 坐标 系 ,并 写 出 对 应 的 D-H 参数 。 

解 : 先 设 定 参 考 坐 标 系 {0} , 它 固定 在 基 座 上 。 根 据 上 述 连 杆 坐标 系 的 建立 原则 可 知 , 当 
关节 变量 1 为 0 时 ,坐标 系 {1} 和 {0} 重 合 ,因此 可 以 建立 如 图 2. 5 所 示 的 坐标 系 {0}, 且 zu 轴 
与 关节 轴 1 的 轴线 重合 。 

该 机 械 臂 位 于 一 个 平面 上 ,因此 所 有 的 z 轴 相 互 平行 , 即 没 有 连 杆 偏 距 ,所 有 的 &; 都 为 
0; 机 械 臂 所 有 的 关节 轴线 都 与 它 所 在 的 平面 垂直 ,所 有 的 x 轴 都 垂直 于 纸 面向 外 ,因此 所 有 
的 w; 也 均 为 0。 

Zi 轴 沿 公 垂 线 a; 的 方向 ,由 关节 轴 ; 指向 关节 轴 i 十 1。 

所 有 y 轴 的 方向 由 右手 法 则 确定 ,各 关节 坐标 系 如 图 2.6 所 示 。 


0 尝 


图 2.5 平面 三 连 杆 机 械 臂 图 2.6 平面 三 连 杆 机 械 臂 的 坐标 系 
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基于 上 述 分 析 和 题 中 的 已 知 条 件 , 可 得 该 平面 三 连 杆 机 械 臂 的 D-H 参数 如 表 2. 1 所 示 。 
表 2.1 平面 三 连 杆 机 械 辟 D-H 参数 表 
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一 且 对 所 有 连 杆 规定 坐标 系 之 后 ,就 可 以 根据 建立 的 关节 坐标 系 , 列 出 各 连 杆 的 常量 参 
数 。 本 节 先 基于 D-H 参数 法 建立 的 关节 坐标 系 推导 出 相 邻 连 杆 之 间 的 坐标 变换 , 即 各 连 杆 间 
坐标 变换 的 一 般 形式 ; 然后 将 这 些 变 换 依 次 相 乘 , 即 可 得 到 机 械 臂 的 正 运动 学 方程 。 

2.3.1 连 杆 变换 矩阵 

对 于 给 定 的 机 械 臂 ,可 以 根据 建立 好 的 关节 坐标 系 按 照 下 列 顺 序 通过 两 次 旋转 和 两 次 平 
移 建立 起 相 邻 连 杆 ;一 1 和: 的 对 应 关系 。 

中 先 将 坐标 系 全 一 1} 绕 >,-1; 轴 旋 转 0; 角 , 再 沿 着 =;_; 轴 平 移 一 段 距离 d; ,使 x;_1 轴 
与 z; 轴 平行 ,得 到 的 新 坐标 系 为 {i 一 1 ) ,对 应 的 轴 分 别 为 x’_1 轴 、y;_1 轴 和 zx’_1 轴 ,对 应 的 
坐标 原点 为 O;_1。 

@ 然后 将 坐标 系 们 一 1 ) 沿 x'_| 轴 平移 一 段 距 离 a,_) ,使 连 杆 ;一 1 和 连 杆 i 的 坐标 原点 
0O;_1 和 0O; 重合 ; 并 绕 x;_|1 轴 旋转 a,_1 角 , 使 坐标 系 {i 一 1 } 和 (i) 的 x 轴 重 合 , 即 z'_| 轴 和 
z; 轴 重 合 。 

经 过 上 述 坐 标 变 换 , 最 终 使 得 坐标 系 {i 一 1) 与 坐标 系 {i} 重 合 ,可 用 表示 连 杆 i 对 连 杆 i 一 1 
相对 位 置 的 4 个 齐 次 变换 矩阵 来 描述 , 称 为 T 和 矩 阵 ,也 称 为 连 杆 变换 和 矩 阵 , 记 为 !T。 按 昭 
“从 左 到 右 ” 的 原则 ,可 以 得 到 ; 'T 的 表达 式 

T=Ry(0,)Trans(0,0,d;)Trans(0,0,a;_ 1)Ry (a,_1) C2 1 
由 式 (2.1) 可 以 看 出 , 连 杆 变换 矩阵 就 是 描述 连 杆 坐标 系 间 相对 平移 和 旋转 的 齐 次 变换 。 对 式 
(2. 1) 展 开 计 算 , 可 得 ; 1T 的 一 般 表 达 式 


0 = 0 Gil 
. 到 
i . | ; (2. 2) 
C27 TE Cai=1d: 
0 0 0 1 


由 式 (2.2) 可 知 , 两 相 邻 坐标 系 之 间 的 变换 矩阵 是 关于 n 个 关节 变量 gq; (i 二 1,2,…,n) 的 函 
数 。 如 果 是 转动 关节 , 则 变量 为 9; ; 若 为 移动 关节 , 则 关节 变量 为 a，,。 | 
2.3.2 运动 学 方程 的 建立 
已 知 连 杆 坐 标 系 和 对 应 的 连 杆 参数 ,将 各 相 邻 连 杆 之 间 的 变换 矩阵 分 别 求 出 ,然后 把 各 连 
杆 变换 矩阵 ; TG 一 1,2,…,m) 顺 序 相 乘 , 可 得 末端 连 杆 坐标 系 {2 } 相对 于 基 坐 标 系 {0} 的 连 
杆 变换 矩阵 ,T 为 
iT = TT T= TO TOO STO De TO, ) (2 
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通常 把 %T 称 为 机 械 臂 的 变换 矩阵 ,显然 , 它 也 是 关于 关节 变量 q; (i 二 1,2,…,n) 的 函数 。 在 
末端 执行 器 坐标 系 和 基 坐 标 系 之 间 关 系 已 知 的 条 件 下 ,如 果 能 够 通过 传感器 测 出 这 些 关节 变 
量 的 值 , 则 机 器 人 未 端 连 杆 在 笛 卡 儿 坐 标 系 下 的 位 姿 ( 即 机 器 人 正 向 运动 学 方程 ) 就 可 以 通 
过 ,T 计算 得 到 。 即 

ri Ti Te By 

PA a (ag | 
Yat Tr tay Ds 01xs 1 

0 0 0 . 

式 (2.4) 中 ,三 行 三 列 的 子 和 矩阵 R 表示 从 基 座 到 末端 执行 器 的 旋转 矩阵 ,其 中 的 每 列 从 左 到 右 
分 别 代表 末端 执行 器 描述 基 坐 标 系 中 X 轴 、Y 轴 和 2 轴 方 向 上 的 单位 矢量 , 即 表示 末端 执行 
器 基于 基 坐 标 系 的 方向 姿态 。 而 三 行 一 列 的 矩阵 已 从 上 往 下 分 别 代表 末端 执行 器 相对 于 基 
坐标 系 的 位 置 。 从 而 可 知 ,基于 D-H 参数 的 齐 次 转换 矩阵 T 的 推导 和 求解 可 以 很 好 地 分 析 机 
器 人 的 正 运动 学 。 


2.4 正 运动 学 方程 举例 


例 2.2 根据 表 2.1 所 示 的 连 杆 参数 ,计算 RRR 机 构 的 运动 学 方程 。 
解 : (1) 先 求 出 各 相 邻 连 杆 之 间 的 变换 矩阵 。 
将 对 应 的 参数 代入 式 (2. 2) 可 得 


(2.4) 


eo = QO 
0 0 0 
| 
0 0 EE 0 
0 0 QL 
ce0, 一 0 | 
s0» 0 0 0 
2T(0,) 一 (2 5) 
0 0 1 
| o | 
[c0, == 0 0 Lia 
50: CO 0 0 
人 的 由 二 | 
0 0 | 从 
0 0 0 


(2) 将 式 (2.5) 中 各 连 杆 的 变换 矩阵 依次 相 乘 ,可 得 机 械 臂 的 正 运动 学 方程 , 即 
Cl23 一 51233 0 LicitL2cy 
S128 cl23 0 LisitL,si 
0 1 0 
: 0 0 0 1 
式 中 ,clz3 王 cos(O 十 0 十 03 ) ,13 一 sin(0 十 0 十 0 ) ,cl 一 cos(0 十 0 )，si 一 sin(0 十 0 )， 


G1 = COSKO 7) 551 = snl Ye 


oT TO TI TS 


22 硬 || 机 器 人 控制 一 一 运动 学 、 控 制 器 设计 、 人 机 交互 与 应 用 实例 


例 2.3 某 个 具有 6 自由 度 机 械 辟 如 图 2.7 所 示 , 它 由 6 个 简化 的 转动 关节 组 成 ,其 D-H 
参数 如 表 2. 2 所 示 。 求 该 6 自由 度 机 械 臂 的 正 运动 学 方程 。 


图 2.7 6 自由 度 机 械 臂 的 坐标 系 
表 2.2 6 自由 度 机 械 辟 D-H 参数 表 


Co 0 
四 0 
9T(01) = |! : 
于 0 0 
0 0 1 
| 
0 52 Cg 0 ss2a2 
Se a GW 1 
L090 0 0 i 
[es = 0 Cods 
2T(0.) 一 A C3 0 ssas 
ra 0 1 “i 
L0 0 下 
Ca Qi caaa | 
5 0 a Sia 
Sm eo 
0 1 0 
lo 0 0 | 
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4 和 5 
1T(05) = 


L0 0 0 a 

| 

C 0 0 
| 

0 1 ri 

0 0 人 也 | 


使 用 以 下 简化 符号 s;.…; 二 sin(0; 十 … 十 0; ) ,ci 一 cosC0 十 … 十 0 ), 则 机 械 臂 正 运动 学 方程 为 : 
7 
0 下 一 9T(9 TOITONTONTONTON 一 | 2 “2 
reat Ta Ta Pa 
0 0 0 1 
式 中 : 
ril 一 cl1(4Cc234C5C6 一 523436) 一 513536 
r21 一 S1(C234C5C6 一 523456) 十 cC13556 


731 一 3234C5C6 523436 


r12 一 一 Cl1(4c234C5356 一 5234C6) 一 35153536 
r22 一 一 51(C234C556 一 3234C6) — C15556 
7 3 一 00346535 一 6234C6 


ris 一 C1C23455 十 5155 
7 2 一 1C28435 人 135 
733 一 223435 
pz =c1(c23444 二 Cc2343 十 czQa2) 
力 , 一 S14c23444 tT c2343 tc2a2) 
ps 一 323444 十 S2343 十 32Q2 
例 2.4 力 触觉 操纵 杆 Geomagic 公司 Touch X 的 结构 如 图 2. 8 所 示 , 它 是 一 个 6 关节 机 
器 人 ,6 个 关节 均 为 转动 关节 。Touch X 的 坐标 系 如 图 2. 9 所 示 , 其 中 0 三/ 一 26.6mm, 其 
D-H 参数 如 表 2. 3 所 示 。 求 其 正 运动 学 方程 。 


4 后 柄 角 生 一 二 一 一 一 


末端 执行 器 
da; 
- | 


图 2.8 Touch X 的 结构 图 2.9 Touch X 的 坐标 系 
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表 2.3 6 自由 度 机 械 臂 Touch X 的 D-H 参数 表 


1 0° 0 0 有 前 
2 一 90” 0 0 0, 
3 0° 26.6 0 03 
5 一 90” 13.3 0 0; 


根据 表 2. 3 和 式 (2.2) ,可 以 得 出 Touch X 机 器 人 各 相 邻 连 杆 间 的 齐 次 变换 矩阵 : 


ET 

5 C & 六 
TO Ys 

0 也 站 

0 0 0 1 

C2 一 3S2 0 0 
1 0 0 U0. 
2T7(0,)= 

~ 一 0 0 

0 0 (9 

ear ss 0 ‘0266 

5 C 0 0 
TOO Es 

0 0 出 0 

0 0 0 1 

Gy gy 0 0 
0 0 1 0.266 
4 工 (04 ) 一 

本 0 

0 0 0 1 

ea $s A —18 
h 0 0 1 0 

TT i 0 0 

0 0 0 1 

2 0 
5 0 0 1 133 
5 TC ) 一 

Se oe. A 0 


0 0 0 1 
式 中 简化 符号 分 别 表示 如 下 : s; 二 sin (0; ),c; 一 cos(0;)(Gi 一 1,2,3,4,5,6) 。 
根据 式 (2. 3) 和 式 (2. 4) ,可 得 Touch X 末端 执行 器 的 位 姿 , 即 Touch X 的 正 运动 学 方程 : 
| rii, Yi “Ve Dy 
rai Woes Toys bg 
Pai To, 3 Ws 


0 0 0 1 


sT =1T(01)3T(02)3T(0s)4T(0,)sT(0s)sT(06) 一 
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ri = els ccs — Ceaea) — Csg Hiee lss (Cnc I OCs Cl (Cesc — cI) ss) 
rol = Se (Cc1C4 十 $4 (Coc35] — $s1s253)) 一 一 
7 
7 
72z = Colc1C4 十 sg(czca8l 一 518253) — Se lss Cosiss tas1ss) 十 ca(cikczcssl — S15253) — G154)) 
an On Sl GG (tod | Cat). — Koda (eo do, Cas 
ris Ge (Crsss | CC Se (Ca (C10s — CS ya) Ts 
ras —Ce Coss FT Cos ) = (ctoca — Sy eo) = Cs) 
ras =Cs(c2c3 — 8253) 十 cass (cyss eass) 
D0 2006rcy — O33 ces.— Cr “0 193 se Ol eicsa | ccasy OO— 
Ql133s0 (C0 (coces — e125s) 315) — 0 ZO6c ic253 — 0. 266¢1.635,» 
psy =0.266c351 ~— 0. 133ca(cscasi — S15288) | 0 L33678a 十 0.13365 (tosis | Casids) — 
0. 133ss (cE (cecasi — BSS) — CS) 0. 200c0313s 一 0 2602 431 3s 
pi =02006s,83 —0.266c0c5 = 0 206sp 0 133 (cacs — Soss) 0 133ca Cc»853 EF cossy 
Q. J33cnse (cose cosy) 
其 中 ,Touch X 的 每 个 关节 都 有 一 定 的 转角 范围 ,具体 如 下 : 
0 三 一 60 一 60"， 0,=0° ~ 105"; 0, = 一 180" 一 180 ， 
0 一 一 145 ~ 145°", 05 =—70° 人 70 0..=—145 ~+"145°。 


2.5 机 械 臂 逆 运 动 学 


为 使 机 械 臂 位 于 期 望 的 位 姿 , 可 通过 逆 运 动 学 求解 以 确定 每 个 关节 的 角度 值 。 前 面 已 对 
机 器 人 着 运动 学 概念 做 了 简单 介绍 。 这 一 节 主 要 研究 求解 道 运动 方程 的 一 般 步 又 。 

2.5.1 关节 空间 与 工作 空间 

对 于 一 个 具有 nn 个 自由 度 的 操作 臂 来 说 , 它 的 所 有 连 杆 位 置 可 由 一 组 ”个 关节 变量 来 确 
定 。 这 样 的 一 组 变量 通常 被 称 为 n X1 的 关节 矢量 。 所 有 关节 矢量 组 成 的 空间 称 为 关节 
空间 。 

机 器 人 的 工作 空间 是 指 机 器 人 末端 执行 器 上 参考 点 所 能 到 达 的 所 有 空间 区 域 。 若 位 置 是 
在 空间 相互 正 交 的 轴 上 测量 , 且 姿 态 是 按照 空间 描述 章节 中 任意 一 种 规定 测量 的 时 候 , 称 该 空 
间 为 笛 卡 儿 空间 ,有 时 也 称 为 任务 空间 或 者 操作 空间 。 

2.5.2 逆 运 动 学 问题 的 多 解 性 与 可 解 性 

机 器 人 正 运 动 学 的 建 模 解决 了 如 何 从 关节 空间 的 关节 位 置 ( 即 关节 角度 ) 求 出 操作 空间 未 
端 执 行 器 的 位 姿 问 题 。 而 机 器 人 的 逆 运 动 学 问题 则 是 将 未 端 执行 器 在 操作 空间 的 运动 变换 为 
在 相应 的 关节 空间 的 运动 ,因此 它 的 求解 更 具有 重要 意义 。 逆 运动 学 的 解 是 否 存在 归根 结 底 
取决 于 机 器 人 的 工作 空间 。 如 前 描述 ,工作 空间 就 是 一 个 机 器 人 的 未 端 执行 器 所 能 到 达 的 范 
围 。 而 求解 道 运动 学 方程 可 能 存在 的 另外 一 个 问题 就 是 解 的 多 重 性 问题 ,具体 有 三 种 情况 。 

(1) 解 不 存在 。 当 所 期 望 的 位 姿 离 基 坐标 系 太 远 ,而 机 械 臂 不够 长 时 ,末端 执行 器 无 法 达 
到 该 位 姿 ; 当 机 械 臂 的 自由 度 少 于 6 个 自由 度 时 , 它 将 不 能 达到 三 维 空间 的 所 有 位 姿 ; 此 外 ， 
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对 于 实际 的 机 械 臂 ,关节 角 不 一 定 能 达到 360" ,这 使 得 它 不 能 达到 某 些 范围 内 的 位 姿 。 以 上 情 
况 下 ,机 械 臂 都 不 能 到 达 某 些 给 定 的 位 姿 , 因 此 不 存在 解 。 

(2) 解 唯 一 。 当 机 械 臂 只 能 从 一 个 方向 达到 期 望 的 位 姿 时 ,只 存在 一 组 关节 和 角 使 得 它 能 
到 达 这 个 位 姿 , 即 存在 唯一 的 解 。 

(3) 存在 多 个 解 。 当 机 械 臂 能 从 多 个 方向 达到 期 望 的 位 姿 时 ,存在 着 多 组 关节 角 能 使 得 
它 到 达 这 个 位 姿 , 即 存在 多 个 解 。 如 对 于 一 个 没有 机 械 关节 限制 的 6 自由 度 机 械 臂 ,通常 有 
16 个 可 行 解 。 此 时 ,我 们 需要 根据 一 些 准 则 来 选择 一 组 最 适合 的 解 : 四 考虑 机 械 臂 从 初始 位 
姿 移 动 到 期 望 位 姿 的 关节 空间 内 的 “最 短 行程 ? 解 ; @ 考 虑 在 机 械 
壁 移动 的 过 程 中 是 否 遇 到 障碍 , 若 遇 到 则 应 选择 无 障碍 的 一 
组 解 。 

某 一 个 三 连 杆 机 械 臂 如 图 2. 10 所 示 , 对 于 某 一 给 定 的 位 姿 , 它 
Ee 有 两 组 解 , 图 中 实 线 和 虚线 各 代表 一 组 解 , 即 多 解 性 ， pe 

i 而 角 函 数 方程 产生 的 。 应 当 根 据 上 述 多 解 情况 下 选择 合适 解 的 一 般 

原则 来 分 析 问 题 ,并 去 除 多 余 解 ,具体 过 程 如 下 。 

1. 根据 关节 运动 空间 限制 选择 解 

例如 , 求 得 某 个 关节 的 两 个 解 分 别 为 

= = 1280 =215 

已 知 该 关节 的 运动 空间 为 土 130", 这 时 应 该 选择 9, = 二 0' = 二 35° 作 为 该 关节 的 角度 值 。 

2. 根据 最 短 行程 选择 最 接近 的 解 

为 使 机 械 臂 在 运动 过 程 中 保持 其 连续 性 与 平稳 性 , 当 它 有 多 个 解 时 ,我 们 应 该 选择 距离 上 
一 时 刻 最 接近 的 解 , 即 每 一 个 运动 关节 的 运动 量 最 小 。 

例如 ,假设 某 个 关节 的 两 个 解 依 然 分 别 为 

b= 
设 定 该 关节 的 运动 空间 为 士 260", 它 前 一 采样 时 刻 0; (n 一 1) = 二 170°, 则 
Ab 一 0 一 0 一 1) 一 35 "一 170" 一 一 135? 
Ab 一 久 一 0 (2 一 1) 一 215" 一 170" 一 45? 

显然 ,Al 更 接近 前 一 时 刻 的 解 ,所 以 应 该 选择 0, 一 多 一 215"。 

3. 根据 避 障 原则 选择 合适 的 解 

如 图 2. 11 所 示 ,机 械 臂 处 于 A 点 ,我 们 希望 它 能 够 到 达 B 点 。 根 据 前 文 描述 的 原则 ,我 
们 应 该 选择 使 关节 运动 量 最 小 的 接近 解 。 如 果 没 有 障碍 物 ， 
按照 这 一 原则 ,我 们 选择 图 2. 11 中 上 面 的 一 条 虚线 所 对 应 的 
解 ; 有 障碍 物 时 ,由 于 障碍 物 的 存在 ,使 得 上 面 一 条 虚线 对 应 
的 解 会 使 连 杆 与 障碍 物 发 生 碰撞 ,这 样 一 来 我 们 就 必须 选择 

一 条 虚线 对 应 的 满足 避 障 要 求 的 解 。 

2.5.3 逆 运 动 学 方程 的 求解 

求解 道 运动 学 方程 时 ,我 们 可 以 从 ,T 开始 求解 关节 角度 。 已 知 ? 矩阵 中 各 个 元 素 的 数 
值 ,用 ,T 左 乘 *“T “矩阵 ,使 方程 右边 不 再 包括 这 个 角度 ,于 是 可 以 找到 产生 角度 的 正弦 值 
和 余弦 值 的 元 素 , 进 而 求 得 相应 的 角度 。 然 后 通过 移 项 以 及 之 前 推导 出 的 齐 次 变换 矩阵 逆 矩 
阵 的 性 质 , 即 可 解 出 逆 运 动 学 方程 。 

假设 "T 中 7 一 6, 具 体 过 程 如 下 : 


图 2.11 满足 避 障 要 求 的 解 
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= TT TO DT TC 


a! 1 
oo BR WY 


Pl | 2 
27T 17T 67 一 67 


2 了 一 11 人 一 10 了 一 1 
3T 2T 17 6 了 一 6 了 


a eg 


BN 0 1 
下 面 结合 具体 的 例子 讲解 求解 过 程 。 


例 2.5 某 6 自由 度 机 械 辟 如 图 2.7 所 示 。 请 根据 其 正 运动 学 方程 求解 对 应 的 关节 


角度 。- 


解 : 例 2. 3 中 求 得 的 6 自由 度 机 械 臂 的 正 运动 学 方程 为 


(1) 求 01。 


"TT TO TO I TO LCA 
ri “Tg Ti By 
ra T2s Tas Py 
ra Ta Wag. Rs 
0 0 0 1 
ril =C1(C234C5C6 一 523456) — $15556 
7r21 =s1(c234C5C6 — $23456) 十 c135C6 


731 一 234C5C6 C23456 


r12 一 一 Cl(《c234C536 一 3234C6) — S15556 
r22 一 一 51(4C234C536 一 5234C6) 一 C13536 
1046 


r13 一 C1C23455 十 S155 
res 一 31162348 C155 
7T33 一 523435 

pr 一 cl1(Cc23444 十 c2343 十 czQ2) 
py 一 S1(c23404 十 cz343 十 czQ2) 


Pz 二 323444 十 S23Q43 十 S2Q2 


为 了 求解 角度 ,从 ;T 开始 ,依次 用 1T 左 乘 上 述 两 个 矩阵 ,得 到 : 
0 


六 


7r22 723 pb 
| 0 
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wi Ol Sy “0 1 
0 = 0 * 时 p 
- | 
0 二 0 汉 rs TS Tss D> > 
0 浊 0 和 0 0 0 
展开 上 式 可 得 本 


rllcl 十 r21s1 7rl2cl 十 r225S1 7l3cl 十 r23S1 Prcit pysi 
731 T's2 r 33 Ds 


a i a 


0 0 0 1 
C234C5C6 一 523456 一 C234C5C6 一 S234C6 C23455 C234Q4 十 c23Q3 十 czQ2 
S234C5C6 十 c23436 一 5234C5C6 十 c234C6 523455 3S23404 十 S23Q3 十 S2Q2 

— 和 5556 C5 0 
0 0 0 | 


式 (2.7) 左 右 两 边 矩 阵 第 3 行 第 4 列 的 元 素 相等 ,有 : 


Dvr FB = 01 =arctan (2 ) ， 01=0] 十 180" 


(2) 求 0 。 
根据 式 (2.7) 左 右 两 边 和 矩阵 第 1 行 第 4 列 元 素 和 第 2 行 第 4 列 元 素 相等 ,可 得 : 


PriciT pysi=c2344 二 Cc2343 十 cza2 


Pp: 一 S234Q4 二 T52343 十 S2Q2 
整理 式 (2.9) 并 对 其 两 边 平方 ,然后 将 平方 值 相 加 ,得 : 
(0 a — comaay” = (cy3a3 my 


“和 2 
(ps —s23444) =(s2343 5s2a2) 


(Pci 大 si 一 cas4Q4) 不 《办 一 5234Q4) = 十 as 十 2apsQ&s(szsasg 十 Cycgs) 


根据 式 (2.9) 的 三 角 函 数 方程 
s01c0, 十 c0150， 一 (CO 十 0,) i 


cO1c0, — s01s0, =c (0 0,)=c1, 
可 得 ,s。sz3 十 cocz3 二 cos[L(0, 十 093) 一 09, 二 cos0,。 
于 是 
a vo) tp soles ,ala 
2ayas 


方程 中 , 除 5234 和 C234 外 ,各 变量 均 已 知 ,s,s 和 C234 将 在 后 面 求 出 。 
已 知 ss3 二 土 V1 一 ca” ,于 是 可 得 : 


C3 


让 
03 一 arctan 一 
0% 


(3) 求 9,,04。 


(2.7) 


(2.8) 


G2..93 


(2. 10) 


(2 11) 


(2.12) 


因为 关节 2、3 和 4 都 是 平行 的 , 左 乘 ?T 和 3T 的 逆 不 会 产生 有 用 的 结果 。 下 一 步 左 乘 


4T~4T 的 逆 , 结 果 为 : 
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a 


天 


pe hg CW TT RAS = 


(2, 13) 
计算 式 (2. 13) 可 得 : 
a CHC8 ”一 5556 3 0 
r21 r22 rg3 六 5 一 0 
5 | 二 C2714) 
rear Ys Ta, Pz 36 C6 0 0 
0 “9 0 1 0 0 0 1 
ri 一 c234(clrll 十 S1r2l) 二 5s2347al 
r21 一 Clr2l 一 S17ll 
r3l 一 一 5234(Clrll 二 $1721) 十 czs4r3l 
r12 一 c234(Clrlz + S1722) 十 S234732 
r22 一 C1722 Lg 
r32=—5234 (C1712 + S1722) 二 C234732 
r13= C234 (C1713 二 51723) 二 $2347 33 
r23=C1723 一 51713 
r33 一 一 S234(clrl3 二 $1723) 十 cza47rss 
方 z 一 caa4 (cl 力 z sipy) 十 sza4 力 。 一 cataz 一 c4Q3 一 Q4 
py,=0 
De Ci Ds sipyy tt erp sos — Sy 
根据 式 (2. 14) 左 右 两 边 矩 阵 的 第 3 行 第 3 列 元 素 相等 ,有 
r33 一 一 5234(Clrls 十 S1723) 十 c234733 一 0 一 
0,34 一 arctan | 0234 =0234 + 180° (2: 15) 


C1713 十 S1723 
由 此 可 计算 s,a 和 ca, 如 前 面 所 讨论 过 的 ,它们 可 用 来 计算 03 。 
再 参照 式 (2. 9) ,并 在 这 里 重复 使 用 它 , 就 可 计算 角 0, 的 正弦 和 余弦 值 。 具 体 步骤 如 下 : 
让 十 pys1 一 cz3444 十 cz343s 十 czQs 


pP: 一 S23444 十 23Q3 十 S2Q2 


由 于 clz 一 cl1c2 一 35152 以 及 下 汪 ,可 得 : 


Pzcipys1—c23444 一 (cacas 一 5253)C2303 十 c242 

(2. 16) 

ps 一 S23444 一 (32c 十 cs3)Q3 十 S2Q3? 

上 面 两 个 方程 中 包含 两 个 未 知 数 ,求解 C2 和 2 ,可 得 : 
> (cas ao) pe sud ) — sawalpac TT bys 一 coda 

A 2 2 2 

(c i es! 
et sl (1 


Er (Caasi as (pci pysi— Cod) TF saas(pbs Sau 


G2 
2 Br 
(csas tt wo) 二 53535253 
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尽管 式 (2.17) 比 较 复 杂 , 但 它 的 所 有 元 素 都 是 已 知 的 ,因此 可 以 这 样 计算 。 


(caa3 a2) (ps —s23404) 一 33Q3( 力 -cl Na — C23404) 


i 《csas Tas) (pyrcit pysi — Cc23404) 二 ssa3 (ps — 523404) a 
既然 9, 和 .03; 已 知 , 进 而 可 得 : 
0 = 0 — D3 = 《2219) 
因为 式 (2. 14) 中 的 9， 有 两 个 解 ,所 以 9, 也 有 两 个 解 。 
(4) 求 9,。 
根据 式 (2. 14) 中 左右 两 边 和 矩阵 的 第 1 行 第 3 列 元 素 和 第 2 行 第 3 列 元 素 分 别 对 应 相等 ， 
可 以 得 到 : 
$52 一 C234(C17r13 二 51723) 二 5234733 
(2. 20) 


CH = Cr Sirti 


Goa CECIr ra Sra sya 
0; 一 arctan 234 1° 18 13 234' 33 (2.21) 
S1713 Cirgg 
(C5 Oss 
式 (2. 14) 左 乘 ${T !, 即 
5 C2 
计算 式 (2. 22) 可 得 : 


LA LA 
Fity YW 0 


0 | 


0 0 
i Pg 0 1 Se ce 0 0 
0 0 
0 + .Wt | 1 


0 0 0 

(C2. 23> 
a 
rin=csLczsa (cirll 十 sirzi) 十 $s234731] 一 ss (sirn = 
LA 

rz=csLczsa ciri2 十 S1722) 十 sz34732] 一 55 (S171s Tg 


LA 
r~21 一 一 5234(Ci7rll 二 $1721) 十 c234731 


r22 一 一 5234(C1712 二 51722) 十 C234732 
根据 式 (2. 23) 中 左右 两 边 矩 阵 第 2 行 第 1 列 元 素 和 第 2 行 第 2 列 元 素 相 等 ,得 到 : 
— 5234 (C1711 十 S17r21) 十 cz347r3l 
Cs 十 S1722) 十 cz34732 机 

至 此 找到 了 6 个 方程 ,它们 合 在 一 起 即 可 知 机 器 人 处 于 任何 期 望 位 姿 的 关节 值 。 虽 然 这 - 
种 方法 仅 适 用 于 给 定 的 机 器 人 ,也 可 采取 类 似 的 方法 来 处 理 其 他 机 器 人 。 

值得 注意 的 是 ,仅仅 因为 机 器 人 的 最 后 3 个 关节 相交 于 一 个 公共 点 , 才 使 得 这 个 方法 有 可 
能 求解 ,否则 就 不 能 用 这 个 方法 来 求解 ,而 只 能 直接 求解 矩阵 或 通过 计算 矩阵 的 逆 来 求解 未 知 
的 量 。 大 多 数 工 业 机 器 人 都 有 相交 的 腕 关节 。 


2.6 本 章 小 结 


本 童 讨论 了 如 何 表示 多 自由 度 机 器 人 在 空间 的 运动 ,以 及 如 何 用 D-H 参数 表 推 导出 机 器 
人 的 正 逆 运 动 学 方程 。 这 种 方法 可 用 于 表示 任何 一 种 机 器 人 的 构 型 ,而 与 关节 的 数量 和 类 型 ， 
以 及 关节 和 连 杆 的 偏 移 和 扭 角 无 关 。 

第 3 章 将 接着 讨论 机 器 人 的 微分 运动 ,实际 等 效 于 机 器 人 的 速度 分 析 。 


机 怖 人 微分 运动 竺 


CHAPTER 3 


第 2 章 讨论 了 机 器 人 各 连 杆 间 的 位 移 关系 ,建立 了 机 器 人 的 运动 学 方程 ,研究 了 运动 学 逆 
解 , 建 立 了 机 器 人 末端 执行 器 的 位 姿 与 关节 矢量 之 间 的 关系 , 即 机 器 人 操作 空间 与 关节 空间 的 
映射 关系 。 

本 章 将 在 位 移 分 析 的 基础 上 进行 速度 分 析 , 人 研究 机 器 人 操作 空间 速度 与 关节 空间 速度 之 
间 的 线性 映射 关系 一 一 雅 可 比 和 矩阵 (简称 “ 雅 可 比 ”)。 雅 可 比 矩 阵 不 仅 用 来 表示 操作 空间 与 关 
节 空 间 之 间 的 速度 线性 映射 关系 ,同时 也 用 来 表示 两 空间 之 间 力 的 传递 关系 。 

微分 运动 指 机 构 ( 机 器 人 ) 的 微小 运动 ,可 以 用 它 来 推导 不 同 部 件 之 间 的 速度 关系 。 机 器 
人 速度 也 称 为 机 器 人 的 微分 运动 ,因此 机 器 人 的 速度 运动 学 也 称 为 机 器 人 微分 运动 学 。 速 度 
运动 学 问题 之 所 以 重要 ,是 因为 机 器 人 不 仅 需要 到 达 某 一 (或 者 一 系列 ) 指 定位 置 ,而 且 还 需要 
按照 给 定 的 速度 到 达 这 些 位 置 。 本 章 先 引入 微分 运动 的 概念 ,然后 对 雅 可 比 矩 阵 的 具体 计算 
方法 进行 介绍 ,并 给 出 具体 实例 。 


3.1 机 器 人 微分 运动 
机 器 人 微分 运动 学 反映 了 机 器 人 关节 空间 与 操作 空间 之 间 的 运动 传递 关系 ,是 实现 机 器 
人 运动 控制 的 基础 。 


3.1.1 雅 可 比 和 矩阵 的 定义 
微分 运动 学 的 主要 目的 是 建立 关节 速度 与 机 器 人 末端 执行 器 线 速度 和 角速度 之 间 的 关 
系 。 若 机 器 人 末端 执行 器 的 线 速度 为 v ,其 角速度 为 w, 关 节 速 度 为 9g 二 (919，,…9,)', 则 关 
节 速 度 的 线性 关系 为 : 
v=J,(g)g (3. 2) 
w =J,, (gq)g Ca 2 
式 中 ,J ,(3Xn) 为 联系 关节 速度 4 和 末端 执行 器 线 速 度 o 的 矩阵 ,J,,(3Xn) 为 联系 关节 速度 
4 和 末端 执行 器 角速度 w 的 矩阵 , 若 机 器 人 末端 执行 器 的 速度 为 圭 , 则 上 式 可 写 为 : 


v ; 
so | -os (3.3) 
w 


式 (3.3) 即 为 机 器 人 的 微分 运动 学 方程 。 其 中 ,6 Xn 的 矩阵 J 称 为 机 器 人 的 雅 可 比 矩 阵 。J 
是 关节 变量 .gq 的 函数 ,也 可 以 表示 为 J(qg)。 换 言 之 , 雅 可 比 和 矩阵 是 机 器 人 末端 执行 器 的 速度 
与 关节 速度 的 线性 变换 矩阵 ,用 公式 表达 如 下 : 


J， 
To- (3. 4) 
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将 末端 执行 器 的 线 速度 (v, ,ov ,z-) 和 角速度 (wo ,wy ,wo-) 6 个 变量 作为 雅 可 比 矩 阵 等 式 
的 左边 , 即 : 


民权 


= (exn) d nxl) (3.5) 


[WwW | (6x1) 

式 (3.5) 中 ,n 代表 机 器 人 的 自由 度 ,J 在 运动 学 中 起 着 非常 重要 的 作用 ,可 以 用 于 对 速度 的 表 
述 。 因 为 所 有 对 速度 的 表述 都 与 线 速度 和 角速度 相关 ,任何 与 速度 有 关 的 表述 都 可 以 和 这 个 
雅 可 比 和 矩阵 建立 联系 。 


3.1.2 旋转 矩阵 的 导数 
由 于 旋转 和 矩阵 具有 正 交 性 ,对 于 时 变 的 旋转 矩阵 R 二 R(t), 可 以 得 到 如 下 关系 : 
RGR' (Gt)=1 
设 O 表示 3X3 的 零 矩 阵 , 对 上 式 求 导 ,可 以 得 到 


RDRICGD) 十 RCGODRICGD) 一 O 


令 

S(t1) =RCD)RIC) (3.6) 
则 有 

SG) 十 SICG) 一 O (7 
所 以 S 为 3X3 反 对 称 和 矩阵 。 将 式 (3.6) 两 边 同时 右 乘 R(t), 可 得 

R(t1) =S()R() 3.. 8) 


即 R(z) 的 导数 可 以 表示 为 它 自身 的 函数 。 

式 (3. 8) 通 过 反对 称 算 子 $ ,将 旋转 矩阵 与 它 自身 的 导数 联系 起 来 。 若 有 任 一 常 向 量 p 和 
向 量 p (i) 二 R(t)p', 则 p(t) 关于 时 间 的 导数 为 

p(t) =R(G)p’ 
将 式 (3. 8) 代 入 上 式 , 整 理 可 得 
p(t)=SCRG)p’ | : 
设 t 时 刻 坐 标 系 R(z) 相 对 于 参考 坐标 系 的 角速度 为 w(z) ,根据 力学 知识 可 得 
p(t)=w(t) X RG)p’ 

综 上 可 以 看 出 ,矩阵 算 子 描述 了 向 量 w 和 向 量 R(i)p' 之 间 的 向 量 积 。 若 向 量 w(1)==(w, ww， 
ww。.) , 则 矩阵 $ 关于 主 对 角 线 对 称 的 元 素 与 向 量 w 分 量 之 间 的 关系 可 表示 如 下 : 


0 2 wy 
$= | ww, 0 一 w, (3.9) 
Fe kL 0 
显然 ,矩阵 S 是 关于 向 量 w(z) 的 函数 , 即 S(t) 二 SC(w(1))。 因 此 , 式 (3.8) 可 以 表示 成 : 
| R(1) =S(w)R() (3. 10) 


式 (3.10) 中 , 若 R 为 旋转 矩阵 , 则 下 式 关 系 成 立 : 
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RS(w)R' =S(Rw) ”3.1 
参考 图 1. 6(a) ,完成 点 P 从 坐标 系 {A} 到 坐标 系 {B} 的 坐标 变换 ,由 式 (1. 10) 可 得 
en i C31) 
对 该 式 求 导 , 则 有 
AP=APporc 十 人 R2P AR?P (3. 13) 
由 式 (3. 8) 旋 转 矩 阵 导 数 的 表达 式 , 可 以 得 到 4^P 与 角度 之 间 的 关系 ,如 下 : 
4 已 一 人 疡 soRc t+ HRSP+SCw")ARP (3. 14) 
令 4R2P 一 Sr, 有 
"P= Po 人 t sR Pw" Xpr (3. 15) 
若 ?P 在 坐标 系 {B} 中 国定 , 即 3P==0, 则 对 式 (3.15) 有 
Pp =Paoge 二 WW 尺 人 入 (3. 16) 


3.1.3， 连 杆 速度 

根据 D-H 参数 法 , 连 杆 ; 关节; 及 其 坐标 系 如 图 3. 1 所 示 。 其 中 连 杆 i 连接 关节 i 和 i 十 1， 
坐标 系 {i} 固 连 在 连 杆 i 上 ,其 原点 在 关节 i 十 1 的 轴 上 ; 而 坐标 系 {i 十 1} 的 原点 在 关节 i 的 
轴 上 。 


图 3.1 机 械 臂 通用 链 i 的 表示 


设 定 坐 标 系 {i 一 1} 和 {i} 的 原点 的 位 置 向 量 分 别 p;_) 为 和 p;; 坐标 系 人 4)} 的 原点 关于 坐 
标 系 {i 一 1} 的 位 置 在 坐标 系 {i 一 1} 中 的 表示 为 ;ir;。 由 式 (3. 10) 可 得 如 下 等 式 : 
pi=piit Riir; , (3.17) 
由 式 (3.16) 可 得 
Pi; =Pb; 1+R,i ‘Er + Wea XR = 于 和 
式 (3.18) 将 连 杆 i 的 线 速度 表示 为 连 杆 i 一 1 的 平 动 速度 和 旋转 速度 的 函数 ,但 是 对 于 不 同 的 
关节 类 型 (转动 型 或 移动 型 ) ,该 表达 式 会 有 所 不 同 。w;_1,; 表示 坐标 系 { 引 } 的 原点 相对 于 坐标 
系 们 一 1) 原 点 的 速度 。 
类 似 地 ,坐标 系 {i} 相 对 于 坐标 系 {i 一 1} 的 角速度 在 坐标 系 {i 一 1) 中 表示 为 w;_1,; ,; 则 根 
据 旋转 合成 表达 式 R; 一 R,_1R; ， 可 得 如 下 关系 : 
S(w,)R; =S(w, i)R,;++R, SC(w, 1 )R’! (3. 19) 
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对 式 (3. 19) ,运用 式 (3. 11) 的 性 质 进行 整理 可 得 如 下 表达 式 : 

WW (3. 20) 
式 (3. 20) 将 连 杆 i 的 角速度 表示 为 连 杆 i 一 1 的 角速度 以 及 连 杆 i 关于 连 杆 i 一 1 的 角速度 的 
函数 。 同 式 (3. 18) 一 样 , 式 (3. 20) 也 与 关节 i 的 类 型 有 关 。 


1) 移动 关节 
当 关 节 i 为 移动 关节 时 ,坐标 系 {i) 关 于 {i 一 1) 的 方向 不 变 , 故 有 : 
Wei =0 03521) 
则 线 速 度 为 
Di—l1,i 一 Qizi 1 《3. 22) 
式 中 ,z,_; 为 关节 i 的 轴 单 位 向 量 。 进 而 式 (3. 18) 和 式 (3. 20) 中 对 应 的 线 速 度 和 角速度 可 分 
别 表 示 成 : 
WW 人 
万 ; 一 万 1 十 dizi 1 十 wi 2 (3.24) 
2) 转动 关节 
对 于 转动 关节 ,角速度 为 
Wa = 《3. 25) 
因为 关节 i 的 运动 引起 了 坐标 系 i 一 1 的 旋转 ,所 以 线 速度 可 表示 为 
Oi (3.26) 
进一步 , 式 (3.18) 和 式 (3. 20) 中 对 应 的 线 速度 和 角速度 可 分 别 表 示 为 : 
Wi = We da (3.27) 
pi = pit Ws Kr 《3. 28) 


3.2 雅 可 比 矩 阵 的 计算 


由 式 (3.5) 可 知 , 雅 可 比 矩 阵 是 一 个 6X?2 的 矩阵 ,可 以 从 线 速度 和 角速度 出 发 进行 求解 。 


3.2.1 对 线 速度 的 作用 

由 前 面 齐 次 转换 矩阵 T 可 知 , 在 笛 卡 儿 坐标 中 ,矩阵 T 的 最 后 一 列 前 三 个 变量 (p,,p,， 
p.) 表 示 末 端 执行 器 或 者 物体 最 后 一 个 关节 坐标 系 相对 于 基 座 参考 坐标 系 的 位 置 ,将 这 3 个 
变量 统一 表示 为 一 个 位 置 变量 x, , 则 线 速度 可 以 表示 为 : 


EE 
Le n 
v= S| (3. 29) 
Wl 1 
gx 
& 9g1 9g， 9q， 


Juigi; 表示 当 其 他 关节 静止 时 ,单个 关节 i 的 速度 对 末端 执行 器 线 速度 的 作用 。 式 (3. 29) 表 
明 , 线 速度 可 以 通过 对 Ju ci; 项 求 和 得 到 。 对 于 不 同 的 关节 类 型 ,J,; 的 具体 表达 式 也 不 
相同 。 
(1) 关节 i 为 移动 关节 , 则 g, 二 4; ,由 式 (3. 22) 可 得 
JE 一 人 iii 
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从 中 可 得 : 


| 
(2) 关节 i 为 转动 关节 ,如 图 3. 2 所 示 。 
由 于 对 线 速度 作用 的 计算 是 相对 于 末端 执行 器 
坐标 系 原点 进行 的 ,所 以 9;J ,; 可 以 表示 为 : 
val = Wei KT = dm KD Did 
从 中 可 得 
J = (ph 


3.2.2 对 角速度 的 作用 
根据 式 (3. 20) ,可 得 


图 3.2 转动 关节 速度 对 末端 执行 器 线 
Wi =W, = DW i = Dd (3.30) 速度 作用 向 量 示 意图 
B= i 
同样 地 ,对 于 不 同类 型 的 关节 ,J,, 的 表达 式 也 不 相同 。 
(1) 关节 i 为 移动 关节 ,由 式 (3. 21) 可 得 


did wi =0 
进而 可 得 
J =0 
(2) 关节 i 为 转动 关节 ,由 式 (3. 25) 得 
a | 
从 中 可 得 
| 
综 上 所 述 ,可 以 将 式 (3.4) 写 成 如 下 形式 : 
J ul di 
J = wh | | (3. 31) 
rp 
其 中 
Zi 1 
加 x 
| (3. 32) 
a bse Ne ks 


上 式 基 于 正和 运动 学 关系 ,给 出 了 一 种 计算 雅 可 比 矩 阵 的 系统 而 简单 的 方法 。 式 (3.32) 中 ,向 量 
xz _1、D， 和 p;_1 都 是 关节 变量 的 函数 。 其 中 ,z;_; 由 旋转 矩阵 R?_1 的 第 3 列 得 到 , 即 
A :A i A (9. 33) 
式 (3,33) 中 ; 汉 s 三 [00 TI 
而 p, 则 由 变换 矩阵 7T 的 第 4 列 的 前 3 个 元 素 得 到 。 将 jp, 表示 成 4X1 的 齐 次 形式 为 
k bP,=IT(g7)*"* !T(g,)po (3. 34) 
其 中 ,po 二 [0 0 1]7。 
pi;-_1 是 由 变换 矩阵)_1T 的 第 4 列 的 前 3 个 元 素 得 到 , 即 
A 0 (3. 35) 
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若 机 械 臂 结构 上 任 一 点 的 正 运动 学 方程 已 知 , 则 式 (3. 31) 可 用 于 计算 沿 机 械 辟 结构 上 任 
一 点 的 平均 速度 和 旋转 速度 。 

需要 注意 的 是 : 雅 可 比 矩 阵 还 取决 于 用 来 表示 末端 执行 器 速度 的 坐标 系 。 上 述 公 式 可 以 
计算 基于 坐标 系 下 的 雅 可 比 矩 阵 。 如 果 要 计算 基于 不 同 坐 标 系 v 表示 的 雅 可 比 矩 阵 , 只 需 知 
道 基于 坐标 系 " 表示 的 旋转 和 矩阵 沿 机 械 壁 结构 上 任 一 点 的 R” 妈 可。 两 坐标 系 中 速度 之 间 的 


关系 如 下 
= | jia 
OO RR" w 


式 中 ,p, 即 为 末端 执行 器 的 线 速度 v ,将 上 式 与 式 (3. 3) 联 立 , 经 整理 可 得 


进一步 可 得 
oe 
J"= J (3..36) 


其 中 ,J” 是 基于 坐标 系 v 表示 的 几何 雅 可 比 矩 阵 。 


3.2.3 和 雅 可 比 和 矩阵 的 逆 

对 于 三 维 空间 运动 的 机 器 人 ,由 前 述 可 知 ,其 雅 可 比 抢 阵 了 是 一 个 6X7? 的 和 矩阵 (>” 为 机 器 
人 的 关节 个 数 )。 当 n=6 时 ,J 变 为 6X6 的 方 阵 , 可 以 直接 求 其 逆 。 

当 J 为 方 阵 且 满 秩 时 ,根据 矩阵 理论 可 得 

本 
BA A 

其 中 ,Adj (J ) 为 了 的 伴随 和 矩阵 ; | 本 | 为 J 的 行列 式 值 ,J 为 关节 角 的 函数 。 

当 |J|==0, 对 应 的 那 组 关节 角 称 为 奇异 点 。 因 为 处 于 奇异 点 时 ,|1J | 二 0, 所 以 雅 可 比 矩 阵 
本 的 逆 不 存在 。 

一 般 情 况 下 ,为 使 机 器 人 在 空间 里 运动 得 更 加 灵活 ,通常 将 关节 数 n 设计 成 大 于 任务 空 
间 自 由 度 mm ,其 中 多 余 的 自由 度 +==n 一 m, 称 为 元 余 自 由 度 。 此 时 ,J 不 是 方 阵 ,对 应 的 雅 可 
比 矩 阵 的 逆 应 该 用 jJ 表示 , 即 伪 逆 。 

同样 地 ,根据 和 矩阵 的 理论 可 得 

tJ) 

其 中 ,J" 表示 J 的 转 置 ,J 为 6Xn 的 矩阵 (n 去 6)。 


3.3 雅 可 比 矩 阵 的 计算 举例 


本 节 将 介绍 一 些 典 型 机 械 臂 的 雅 可 比 矩阵 的 计 
算 ,个 别 例子 在 前 面 章节 中 已 计算 过 其 正 运动 学 方程 ， 
因此 正 运 动 学 的 表达 式 将 直接 给 出 。 

3.3.1 平面 三 连 杆 机 械 禹 


平面 三 连 杆 机 械 辟 如 图 3. 3 所 示 , 已 知 其 正 运 动 
学 方程 为 . 图 3.3 平面 三 连 杆 机 械 辟 


《3..37) 
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cl23 一 51233 0 Licl 十 /zclz 十 /3cl123 
sl cl13 0 .451 十 /2s12 十 /35123 
叶 1 0 
0 1 
求 其 雅 可 比 矩 阵 。 
解 : 由 于 平面 连 杆 的 3 个 关节 均 为 转动 关节 ,根据 式 (3. 32), 可 得 雅 可 比 和 矩阵 为 
WS zi XPs — Pi) | 
J(g)= 
Zo 21 Zz2 


不 同 连 杆 的 位 置 向 量 分 别 计算 如 下 : 


0 lici licit lc 
po= |0 bi = | pi= |lisit ls 
0 0 0 


Lici tt loc tt lsc 
po= |lisit Ll2s1 tt Las123 
0 
由 于 所 有 转动 关节 轴 的 单位 向 量 都 平行 于 轴 z。 ,可 得 


0 
zo 一 Z1 一 2Z? 一 |0 
1 


由 式 (3. 31) 得 


一 151 一 /25s12 las 一 12512 一 135123 — lss123 
Lici tt loc tt Lc lc12 十 ZL3cl123 /43c123 
es 0 0 0 
0 0 0 
0 0 0 
1 1 1 


3.3.2 拟人 机 械 辟 
如 图 3.4 所 示 为 三 自由 度 的 拟人 机 械 臂 ,已 知 其 正 运动 学 方程 为 


CC ~ CIS 51 ci(l2cs 十 13c23) 
pg SCs Wyo —Uy Slats te ess) 
” 523 C23 0 L252 tT L352 

0 0 0 1 


求 其 雅 可 比 和 矩阵 。 外 
解 : 根据 拟人 机 械 臂 的 关节 类 型 ,由 式 (3. 32) 可 得 雅 可 比 和 矩阵 为 
Ws zi X (ps— Pi) Bi 
J(g)= 
Zo &1 Z2 


计算 各 连 杆 的 位 置 向 量 得 
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图 3.4 三 自由 度 拟 人 机 械 臂 


0 L1c1c2 
po=pi= |0 Ds= Lisics 
0 


L252 
| ci(lscs 十 13c23) 
ps= |s1(l2cz Laczs) 
L252 十 /3s23 
计算 各 转动 关节 轴 的 单位 向 量 得 


由 式 (3. 31) 得 
一 
clkGlscy 十 13c23) SC 
Se 0 /2c2 十 13c23 /43c23 
人 0 51 51 
0 一 4 
1 0 Won 


3.3.3 球形 腕 机 械 辟 
三 自由 度 球形 脐 机 械 臂 如 图 3.5 所 示 , 它 由 3 个 相互 垂直 且 相 交 的 关节 组 成 。 已 知 其 正 


运动 学 方程 为 
S153 十 clczc3 到 一 
oT 一 一 了 13 
SoC $5253 i 
0 0 ,$ 医 1 
求 其 雅 可 比 矩 阵 。 


解 : 根据 球形 腕 机 械 臂 的 关节 类 型 ,由 式 (3. 32) 可 得 雅 可 比 矩 阵 为 
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图 3.5 三 自由 度 球形 腕 机 械 臂 


zo XK (pa po rthe =p ZX pa pea) 
J(g)= 
Zo 和 2 


计算 各 连 杆 的 位 置 向 量 得 


ci1(lzcs 十 13c23) 
po= |s1(l2c2 十 /sc23) 
L252 十 /ss23 
计算 各 转动 关节 轴 的 单位 向 量 得 


0 | | 9 一 C132 
5 一 5 | Er Z2 二 | 一 5132 
二 | l Tug 
由 式 (3. 31) 得 
[asins — rica 了 ] 
-二 本 
站 0 dasa 0 
0 = = 
0 cl — 
A 0 一 


3.4 本 章 小 结 


本 章 主要 讨论 了 雅 可 比 矩 阵 的 定义 和 求解 方法 ,在 此 基础 上 ,给 出 了 求解 雅 可 比 和 矩阵 的 几 
个 实例 。 第 4 章 将 讨论 机 器 人 的 动力 学 建 模 。 


机 融 人 动力 学 建 模 


CHAPTER 4 


对 于 一 个 机 器 人 ,其 运动 学 指 的 是 该 机 器 人 的 末端 执行 器 所 处 坐标 系 的 位 姿 与 基 座 参考 
坐标 系 之 间 的 关系 ,这 个 过 程 中 涉及 各 关节 的 位 姿 、 速 度 (包括 线 速度 和 和 角速度) 等 ,但 从 未 涉 
及 引起 机 器 人 运动 的 力 。 本 节 将 具体 研究 机 器 人 的 动力 学 方程 一 一 由 驱动 器 施加 的 力矩 或 者 
作用 在 机 械 臂 上 的 外 力 使 机 器 人 运动 的 描述 。 

如 第 2 章 所 分 析 , 定义 机 器 人 关节 角 矢 量 为 g, 对 于 众多 串联 型 6 关节 机 器 人 (包括 
Touch X 机 器 人 ) ,可 以 由 下 式 表示 : 

q={01 0, 03 0 05 06} (4. 1) 

对 于 这 种 机 器 人 的 动力 学 分 析 ,主要 研究 期 望 关 节 力 矩 r 和 已 知 的 轨迹 点 q、g 和 4 之 间 
的 关系 ,由 这 种 关系 推导 出 的 动力 学 公式 与 机 器 人 控制 方式 紧密 相关 。 相 反 , 当 已 知 施加 在 关 
节 上 的 一 组 力矩 r 时 ,计算 机 械 臂 的 关节 角 矢 量 gd 和 9, 这 对 于 机 器 人 的 仿真 很 有 帮助 。 

一 般 有 两 种 方法 用 于 对 机 器 人 动力 学 模型 的 构建 ,一 种 是 基于 速度 .加 速度 和 力 的 牛顿 - 
欧 拉 法 , 另 一 种 是 基于 能 量 的 拉 格 朗 日 法 。 由 这 两 种 方法 得 出 来 的 动力 学 方程 都 可 以 对 动力 
学 的 状态 空间 方程 进行 简化 。 接 下 来 将 分 别 讨论 这 两 种 方法 。 


4.1 用 牛顿 - 欧 拉 法 建立 机 器 估 动 力学 方程 


本 节 重 点 讨论 基于 牛顿 ; 欧 拉 法 建立 机 器 人 动力 学 方程 。 


4.1.1 机 器 人 刚体 的 加 速度 
两 个 相互 独立 的 坐标 系 {A} 和 {B), 坐 标 系 {B} 固 连 在 一 个 刚体 上 ,刚体 有 一 个 相对 于 坐 
标 系 {A} 的 运动 点 ?Q ,如 图 4. 1 所 示 ,假设 坐标 系 {A} 是 固定 的 。 坐 标 系 {B) 相 对 于 坐标 系 
{A) 的 位 置 可 以 用 位 置 撩 量 ^Pporc 和 旋转 矩阵 3R 来 描述 。 则 Q 点 在 坐标 系 {A } 中 的 线 速度 
可 以 表示 为 : 
4Va = Wpone + SR Vo : CR 
注意 : 上 式 成 立 的 前 提 是 坐标 系 {A} 和 {BB) 相 对 方位 保持 一 定 。 
普通 情况 下 , 即 机 器 人 均 是 转动 关节 的 时 候 ,Q 点 在 {B) 坐 标 系 中 的 位 置 固定 , 即 ”Q 为 常 
量 的 时 候 ,关节 转动 时 ,坐标 系 {B} 相 对 于 坐标 系 {A ) 的 旋转 的 角速度 为 ^Qs。 经 过 推导 和 计 
算 ,最 后 得 到 机 器 人 的 线 加 速度 的 表达 式 为 : 
My = son + 0 关 全 DR ON 二 人 AR O (4. 3) 
通常 情况 下 ,上 和 式 用 于 计算 转动 关节 机 械 臂 连 杆 的 线 加 速度 。 
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图 4.1 坐标 系 {B} 相 对 于 坐标 系 {A) 以 速度 人 Vporc 平移 


同上 假设 ,关节 转动 时 ,坐标 系 {B} 相 对 于 坐标 系 {A ) 的 旋转 的 角速度 为 $0 ,而 坐标 系 
{C} 相 对 于 坐标 系 {B) 的 旋转 的 角速度 为 3Q. , 则 坐标 系 {C} 相 对 于 {A ) 旋 转 的 角速度 为 : 
N00.=405+ SR De (4. 4) 
对 其 求 导 , 最 终 得 到 : 
A0. =<0s + SR OT Os KAR DE (4. 5) 
由 上 式 即 可 计算 机 械 臂 连 杆 的 角 加 速度 。 
4.1.2 机 器 人 刚体 的 质量 分 布 
分 析 机 器 人 动力 学 时 ,还 应 考虑 机 器 人 刚体 的 {4} 
质量 分 布 。 对 于 转动 关节 机 械 臂 ( 定 轴 转 动 ) ,在 一 3 
个 刚体 绕 任意 轴 作 旋转 运动 的 时 候 , 用 惯性 张 量 表 
示 机 器 人 刚体 的 质量 分 布 。 如 图 4. 2 所 示 ,在 刚体 ‘0 
上 建立 一 个 坐标 系 {A), 并 用 左上 标 表 示 惯 性 张 量 
所 在 的 参考 坐标 系 , 则 坐标 系 {A } 中 的 惯性 张 量 可 


表示 为 一 个 3X3 的 矩阵 : 4 
Li yn La 图 4.2 坐标 系 {A} 中 的 惯性 张 量 ( 图 中 ^P 
Ee 3 (4. 6) 表示 单元 体 dva 的 位 置 矢量 ) 
= ss] I z 
其 中 的 各 元 素 分 别 为 : 
i = 则 cy 十 z?)pdv (4.7) 
V 
7 = 站 十 zx)odv (4. 8) 
V 
和 = 攻 jc + ypao (4.9) 
V 
站 = 站 -eav (4. 10) 


下 = 站 -av (4..11) 
V 
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Le = yzpas (4. 12) 

V 
式 中 ,机 器 人 刚体 由 微分 体积 dv 组 成 ,其 密度 是 人 
上 面 6 个 相互 独立 的 元 素 的 大 小 取决 于 所 在 坐标 系 的 位 姿 。 其 中 ,了 7,， ， 称 为 惯量 


矩 , 其 余 3 个 称 为 惯量 积 ,参考 坐标 系 的 轴 称 为 主轴 ， es 


4.1.3 ”牛顿 - 欧 拉 递 推动 力学 方程 
将 机 器 人 上 的 连 杆 看 作 刚 体 , 首 先 确定 机 器 人 每 个 连 杆 的 质量 分 布 ,包括 质心 位 置 和 惯性 
张 量 。 使 连 杆 运动 的 前 提 是 可 以 对 连 杆 进行 减速 和 加 速 , 即 连 杆 运动 所 需 的 驱动 力 是 关于 连 
杆 的 期 望 加 速度 和 质量 分 布 的 函数 。 而 牛顿 - 欧 拉 方 程 就 是 描述 了 力 或 力矩 与 惯量 .加 速度 等 
之 间 的 关系 。 
根据 牛顿 第 二 定律 , 即 物体 加 速度 的 大 小 与 作用 力 成 正比 ,与 物体 的 质量 成 反比 ,可 以 得 
到 机 器 人 中 连 杆 质心 上 的 作用 力 下 与 相对 应 的 刚体 加 速度 的 关系 式 : 
F 一 mb。 (4. 13) 
其 中 ,m 是 刚体 的 总 质量 
而 对 于 一 个 转动 的 刚体 ,还 要 分 析 引 起 刚体 转动 的 力矩 N 。 欧 拉 方 程 用 来 表示 作用 在 刚 
体 上 的 力矩 与 刚体 转动 的 角速度 和 角 加 速度 的 关系 : 
太一 “1 十 四 XIo (4. 14) 
上 式 中 ,5T 指 刚体 在 坐标 系 {C} 中 的 惯性 张 量 。 注 意 刚 体 的 质心 的 位 置 (位 于 坐标 系 原 点 ) 。 
有 了 上 面 两 个 方程 ,可 以 进一步 得 到 基于 机 械 臂 给 定 运动 轨迹 求解 驱动 力 或 力矩 的 方法 ， 
即 已 知 关节 的 位 姿 , 速 度 和 加 速度 分 别 为 Q .4 和 4 ,可 以 进一步 得 出 机 器 人 运动 的 驱动 力 。 
这 种 计算 方法 可 分 为 两 步 。 
第 一 步 , 在 已 知 连 杆 位 置 gq 的 情况 下 ,从 连 杆 1 到 连 杆 n 向 外 递 推 计算 连 杆 的 速度 4 和 
加 速度 % ,然后 进一步 对 机 器 人 的 所 有 连 杆 使 用 牛顿 和 欧 拉 方程 ,得 到 作用 在 连 杆 质心 上 的 力 
和 力矩 。 对 于 转动 关节 , 递 推 求解 的 具体 过 程 如 下 : 


wih 全 及 ai Bia Zin (4. 15) 
Hin 5 人 有 Ri 下 人 有 Rawi x Oi Zin 十 bi WA (4. 16) 
Mo = RC WP wy Xs KPa "ds) (C4. 17 
Tv =F RO oO XPo 十 wn X(T wn X Po) br) (4.18) 
TF =min” be, | CA TO 
Ni 0 | in Eh "win Wn TH wi (4. 20) 


式 (4. 15) 一 式 (4. 20) 中 ,i 二 0,1,2,3;4,5。 对 于 6 个 关节 都 是 转动 关节 的 机 器 人 ,通过 上 
面 的 式 子 可 以 求解 出 作用 在 每 个 连 杆 上 的 力 和 力矩 。 

第 二 步 ,计算 关节 力气 。 实 际 上 ， 这 些 关节 力矩 是 施加 在 连 杆 上 的 力 和 力矩 即 动力 学 要 
得 出 的 驱动 器 施加 在 机 器 人 上 的 力矩 或 作用 在 机 器 人 上 和 使 其 运动 的 外 力 。 而 这 种 求解 需要 使 
用 向 内 递 推 的 方法 ,在 得 到 上 面 的 结果 后 ,具体 过 程 如 下 : 


fi=mnR™ fentF, (4. 21) 
Es Ni 本 Rt np ‘Pe XF, 证 Pin XinR™ fin (4. 22) 
人 


a (4. 23) 
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式 中 ,=6,5,4,3,2,1。 式 (4.21) 一 式 (4. 23) 即 为 通过 牛顿 - 欧 拉 递 推 法 推导 得 出 的 机 器 人 动 
力学 方程 。 

在 分 析 机 器 人 动力 学 的 过 程 中 ,还 有 一 个 因素 不 能 忽视 , 即 重力 因素 。 需 要 将 各 连 杆 的 重 
力 加 入 到 动力 学 方程 中 ,由 于 在 递 推 推导 过 程 中 ,计算 力 的 时 候 使 用 到 连 杆 的 质量 和 加 速度 ， 
所 以 可 以 假设 机 器 人 正 以 lg 的 加 速度 向 上 做 加 速 运动 ,这 和 连 杆 上 的 重力 作用 是 等 效 的 。 
所 以 可 以 让 线 加 速度 的 初始 值 与 重力 加 速度 大 小 相等 ,方向 相反 。 这 样 将 不 需要 进行 其 他 附 
加 的 运算 就 可 以 把 重力 的 影响 加 入 到 动力 学 方程 中 。 

上 面 各 式 即 为 运用 牛顿 - 欧 拉 递 推 的 方法 通过 机 器 人 的 运动 轨迹 (即位 姿 . 速 度 和 加 速度 ) 
得 到 机 器 人 的 期 望 驱动 力矩 的 具体 过 程 。 其 中 ,角速度 、 角 加 速度 和 线 加 速度 的 初始 值 分 


” 别 是 : 
0 0 一 
=- 人 | 0 (4. 24) 
0 0 0 


4.2 用 拉 格 朗 日 法 建立 机 器 人 动力 学 方程 


4. 1 节 讨论 了 基于 牛顿 - 欧 拉 法 建立 机 器 人 动力 学 模型 ,本 节 将 讨论 另 一 种 建立 机 器 人 动 
力学 模型 的 方法 拉 格 朗 日 法 。 

4.2.1 状态 空间 方程 

如 果 能 够 对 方程 进行 归纳 和 分 类 ,然后 进一步 简化 ,就 可 以 很 简便 地 表示 机 器 人 的 动力 学 
方程 。 其 中 有 一 种 就 是 用 状态 空间 方程 表示 动力 学 方程 。 不 考虑 一 切 摩擦 因素 ,其 具体 形式 
如 下 : 


r=M(g)g +V(g,9) +G(g) (4. 25) 

式 中 适用 于 前 文 经 常 分 析 的 6 关节 ( 均 是 转动 关节 ) 机 器 人 ,M(g) 是 机 械 臂 的 6X56 惯性 矩阵 ， 
该 矩阵 是 一 个 角 对 称 和 矩阵 ,在 这 个 6X6 矩阵 中 ,里 面 的 非 零 元 素 的 大 小 取决 于 机 器 人 中 各 关 
节 角 4(0 ,0,,0;,04,0; 和 .04) 的 大 小 。M(a) 表 示 机 械 臂 受到 的 惯性 力 的 大 小 。V(a,a ) 为 
6X1 的 离心 力 和 科 里 奥 利 ( 科 氏 力 ) 矩 阵 , 该 矩阵 中 非 零 元 素 的 大 小 取决 于 两 个 因素 机 器 
人 中 各 关节 的 关节 角 g 及 其 关节 角速度 4。G(g) 是 6X1 重力 矩阵 , 即 机 械 辟 上 各 连 杆 的 重 
力 因 素 ,表示 了 这 个 机 器 人 受到 重力 的 大 小 。G(g) 中 非 零 元 素 的 大 小 与 机 器 人 各 关节 的 关节 
角 g 有 关 。 这 里 说 的 理想 情况 是 不 考虑 关节 之 间 的 摩擦 问题 等 其 他 因素 。 

由 于 上 式 中 的 离心 力 和 科 氏 力矩 了 泗 V(g,9) 分 别 取决 于 机 械 臂 各 关节 连 杆 的 位 置 和 速 
度 , 所 以 将 这 个 方程 式 称 为 状态 空间 方程 。 

4.2.2 拉 格 朗 上 日 法 

牛顿 - 欧 拉 法 是 通过 基于 由 牛顿 定律 和 欧 拉 方 程 推导 出 作用 在 连 杆 上 的 力 和 力矩 的 方法 
得 到 机 器 人 动力 学 的 方法 。 而 拉 格 朗 日 法 则 是 基于 能 量 的 角度 来 分 析 机 器 人 的 动力 学 。 对 于 
同一 个 机 器 人 ,两 者 得 到 的 动力 学 方程 都 是 相同 的 。 

首先 从 分 析 动 能 开始 进行 能 量 分 析 , 对 于 机 器 人 的 第 i 个 连 杆 ,其 动能 可 以 表示 为 : 


I 
=Fmivt ve, + ol To (4. 26) 


上 式 中 等 号 右 侧 的 两 项 分 别 代 表 由 连 杆 的 线 速度 (质心 处 ) 引 起 的 动能 和 由 连 杆 的 角速度 
( 同 为 质心 处 ) 引 起 的 动能 。 整 个 机 械 臂 的 动能 是 所 有 连 杆 的 动能 之 和 , 即 : 
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k= Dk (4. 27) 


而 机 器 人 的 动能 又 可 以 和 之 前 的 惯性 矩阵 M(g) 建 立 等 式 ， 对 于 6 关节 机 器 人 ,6 连 杆 的 动能 
可 以 由 6X6 和 矩阵 M(g) 与 关节 角速度 了 建立 关系 式 : 


kl(g,9) =79°M(g)Y (428 


从 物理 力学 可 知 ,物体 的 总 动能 总 是 为 正 值 , 所 以 惯性 矩阵 M(a ) 为 正定 和 矩阵。 
然后 研究 机 器 人 的 势能 。 对 于 机 器 人 的 第 i 个 连 杆 ,其 势能 可 以 表示 为 : 
ui 一 一 六 18 "Pe, + we, (4. 29) 
上 式 中 的 "g 是 3X1 的 重力 加 速度 矢量 ,而 "Pe 则 是 第 i 个 连 杆 的 质心 的 相对 位 置 矢量 ， 
而 为 了 使 势能 最 小 为 0, 取 一 常数 为 wref 。 则 整个 机 械 辟 的 势能 是 所 有 连 杆 的 势能 之 和 , 即 : 


u = Du; (4. 30) 
i 一 1 


因为 "Pc 是 第 ; 个 连 杆 的 质心 的 相对 位 置 矢 量 , 则 "Pc 应 该 是 关节 角 的 函数 。 则 机 械 辟 
的 整体 势能 可 以 表现 为 u(g), 它 是 各 关节 位 置 的 标量 函数 。 
当 得 到 机 器 人 的 动能 和 势能 后 ,进一步 推导 计算 得 到 拉 格 朗 日 函数 , 即 : 
L(g,g)=k(g,g)—u(g) (4. 31) 
通过 拉 格 朗 日 函数 得 到 机 器 人 的 驱动 力矩 : 
2 
ee (4. 32) 
对 于 机 械 辟 ,方程 式 也 可 以 这 样 表 示 : 
hs 和 C4.33 
这 样 通过 计算 机 器 人 的 动能 和 势能 ,再 代入 拉 格 朗 日 函数 中 整理 ,可 得 机 器 人 的 动力 学 方程 ， 
这 就 是 由 拉 格 朗 日 法 推导 机 器 人 的 动力 学 。 


4.3 机 器 人 动力 学 建 模 举例 


Touch X 为 6 自由 度 的 关节 机 器 人 ,为 触摸 式 人 机 交互 的 力 反馈 设备 ,是 模仿 人 的 手臂 进 
行 设 计 的 ,如 图 2.8 所 示 。 因 此 本 书 中 也 称 Touch X 机 器 人 为 机 械 臂 。 | 

4. 2 节 通 过 方程 简化 和 整理 ,将 机 器 人 的 动力 学 方程 构建 成 关节 空间 方程 。 这 是 因为 关 -~ 
节 空 间 方程 不 仅 易 于 表达 机 器 人 的 动力 学 关系 ,而 且 对 于 串联 型 机 器 人 还 有 利于 利用 其 串联 
结构 的 性 质 推 导 动 力学 方程 ; 而 Touch X 则 是 典型 的 6 关节 串联 结构 机 器 人 。 

对 于 关节 空间 方程 ,要 获得 各 矩阵 的 具体 值 或 表达 式 , 则 需要 大 量 的 计算 ,其 中 有 一 种 方 
法 称 为 了 Y 和 矩阵 (匹配 ) 法 。Y 和 矩阵 法 就 是 建立 在 状态 空间 方程 的 基础 上 得 到 的 方法 ,本 节 将 使 
用 Y 立 矩阵 法 来 构建 Touch X 的 动力 学 模型 。 

先 将 关节 空间 方程 转化 为 以 下 方程 : 

M(g)g+V(g,9)++G(g)=Y(g,9,9)o (4 34) 
式 中 ,o 包括 系统 变量 ,而 Y 矩阵 中 都 是 关节 角 矢 量 有 关 的 函数 (关节 角 、 关 节 角 速度 、 关 节 角 
加 速度 ) 。 这 种 方法 的 思路 是 : 先 将 了 矩阵 求解 出 来 ,然后 分 离 同 类 项 ,分 别 和 上 面 方程 左 部 
的 三 项 相对 应 ,最终 得 到 机 器 人 的 动力 学 关节 空间 方程 。 
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A 


现在 假设 期 望 理想 轨迹 的 关节 角 矢量 为 g, , 则 轨迹 误差 控制 可 以 用 以 下 方程 式 表示 : 
qe =qg—gqya (4. 35) 
s=0, 二 Ag, = 和 一 0， (4. 36) 
式 中 ,gq, 二 94 一 A $9, ,而 A 是 一 个 对 称 正 定 矩阵 。 通 过 一 定 的 数学 公式 变换 以 及 推导 ,动力 学 
关节 空间 方程 可 以 进一步 转换 为 : 


M(g)g, +V(g,9)9, +G(g)=Y(g,9,9,,9,)0 = 三， (4. 37) 
下 面 定义 一 些 矢量 . 

a; = (0 49,) — 0,) — (9) + 36;C0) (4. 38) 
Bi; = (0 +9,) — (9,) — og) + 3%,(0) (4. 39) 
K;=F,(9 +94,) —F,(g,)—F,(g) + 3F,(0) (4. 40) 
U, =N,(G 49,)— N;(g,)— N,(g)+3N,(0) (4.41) 

对 于 转动 关节 ,力矩 的 第 i 个 元 素 可 以 表示 为 : 
Er 证 = (4. 42) 


之 后 可 将 g, 这 个 动力 学 矢量 ,以 及 关系 式 w; (9 十 9,) 二 w; (9,) 十 w;(9), 代 入 牛顿 - 欧 拉 
递 推 公式 ,可 以 将 上 述 定 义 过 的 4 个 矢量 推导 为 递 推 的 形式 ,具体 过 程 与 上 面 牛顿 - 欧 拉 递 推 
公式 类 似 , 在 此 不 再 缆 述 。 


在 推导 过 程 中 ,定义 以 下 两 个 变量 : 
站 一 [oa Xo (Gg) XI (Low, (8,) Xo C8) XD) (4. 43) 
$, =T,; 十 Lao; XJ (4.44) 
对 于 第 i 个 连 杆 ,将 系统 变量 o 设置 为 : 
es WE PE EY PO EY er (4. 45) 
然后 矢量 K, 和 U, 可 以 表现 为 : 
;三 证 RR (4. 46) 
t= Boy | 1 RCP 0 十 Un ) (4. 47) 
其 中 ， 
A;:= [8: ®$, do (4. 48) 
;二 而 一 PB i] (4. 49) 


9 I eC A I 0 A 
W(X Tw 0) 
现在 分 析 Y 矩阵 。Y 矩阵 可 以 如 下 表示 : 
i 


2 2 
人 (4. 50) 


Ynl ee VY nn 
在 得 到 和 矩阵 中 每 个 非 零 元 素 的 表达 式 之 前 ,需要 先 明确 几 个 变量 。 
对 于 i=1,2… yn sy 令 =ii 十 1 yn; 则 ， 


大 
TE 1 (4. 51) 


j=i 


46 者 机 器 人 控制 一 一 运动 学 、 控 制 器 设计 、 人 机 交互 与 应 用 实例 


] 


x = Ry (4. 52) 
ye ~ My 
则 对 于 转动 关节 ,Y 矩阵 中 的 非 零 元 素 可 以 表示 为 : 
yn = Bt+yt AAA (4. 54) 
最 后 可 得 Y 矩阵 中 各 元 素 的 表达 式 。 和 


4.4 ”本 章 小 结 


本 章 具 体 分 析 了 机 器 人 的 动力 学 , 即 机 器 人 末端 执行 器 的 运动 轨迹 (oa 和 4) 和 驱动 器 
施加 的 力矩 或 者 作用 在 机 械 臂 上 的 外 力 之 间 的 关系 。 分 析 动 力学 有 两 种 方法 ,一 种 是 牛顿 - 欧 
拉 法 , 另 一 种 是 拉 格 朗 日 法 。 其 中 ,牛顿 - 欧 拉 法 基于 生 顿 第 二 定律 和 欧 拉 法 ,通过 递归 方程 求 
解 机 器 人 的 动力 学 方程 ; 而 拉 格 朗 日 法 是 基于 能 量 的 基础 得 到 机 器 人 的 动力 学 方程 。 可 以 用 
状态 空间 方程 来 简化 机 器 人 的 动力 学 方程 ,得 到 机 器 人 状态 空间 方程 上 的 几 个 和 矩阵 (惯性 矩 
阵 、 科 氏 力 矩阵 和 重力 矩阵 ) 的 表达 式 。 


机 器 人 轨迹 规划 


CHAPTER 5 


当 指 定 机 器 人 执行 某 项 操作 时 ,往往 会 附带 一 些 约束 条 件 ,如 要 求 机 器 人 从 空间 位 置 A 
沿 指 定 路 径 平 稳 地 到 达 位 置 B。 这 类 问题 称 为 对 机 器 人 轨迹 进行 规划 和 协调 的 问题 。 前 面 章 
节 完 成 了 对 机 器 人 的 运动 学 建 模 和 动力 学 建 模 ,在 此 基础 上 ,本 章 将 对 机 器 人 的 轨迹 规划 进行 
研究 ,主要 包括 关节 空间 和 直角 坐标 空间 ( 笛 卡 儿 空 间 ) 机 器 人 的 轨迹 规划 。 

机 器 人 在 作业 空间 ( 即 操作 空间 ) 要 完成 给 定 的 任务 ,其 末端 执行 器 的 运动 必须 按 一 定 的 
轨迹 进行 。 机 器 人 的 轨迹 是 指 机 械 臂 在 空间 中 的 期 望 运动 ,在 本 书 中 ,轨迹 指 的 是 机 器 人 中 每 
个 关节 的 位 置 .速度 和 加 速度 在 一 段 时 间 内 的 变化 ,机 器 人 的 轨迹 规划 是 机 器 人 控制 的 第 一 
步 。 在 具体 设计 机 器 人 的 轨迹 时 ,理想 情况 是 允许 用 户 只 用 相对 简单 的 描述 就 可 以 控制 机 器 
人 的 运动 轨迹 ,然后 由 系统 确定 到 达 目 标 位 置 的 最 优 路 径 \ 所 用 时 间 、 速 度 和 加 速度 等 。 而 不 
是 让 用 户 自身 必须 写 出 复杂 的 时 间 和 空间 的 函数 才能 指定 机 器 人 的 期 望 运动 。 

轨迹 规划 要 解决 的 问题 是 将 机 械 辟 从 初始 位 置 运 动 到 一 个 期 望 终点 位 置 ,运动 过 程 包括 
机 器 人 中 各 关节 所 在 坐标 系 相 对 于 基 座 坐标 系 的 位 姿 变 换 , 即 同时 包括 位 置 和 姿态 变化 。 对 
于 这 种 变化 过 程 ,不 是 只 包括 两 个 位 置 ( 即 初始 位 置 和 期 望 终 点 位 置 ) ,而 应 该 在 整个 路 径 中 设 

一 系列 期 望 中 间 点 ,这些 中 间 点 位 于 初始 位 置 和 终点 位 置 之 间 ,通过 这 些 中 间 点 组 成 一 个 针 
对 具体 运动 的 期 望 路 径 。 实 际 上 ,这 些 中 间 点 都 是 期 望 运 动 过 程 中 各 关节 所 在 坐标 系 相 对 于 
基 座 参考 坐标 系 的 位 姿 ,而 非 通常 意义 中 的 “点 ”。 


5.1 机 器 人 轨迹 规划 概述 


5.1.1 运动 .路 径 和 轨迹 规划 

为 避免 大 家 混淆 运动 规划 、 路 径 规划 和 轨迹 规划 的 概念 ,下 面 将 分 别 进行 解释 。 

运动 规划 由 路 径 规划 (空间 ) 和 轨迹 规划 (时 间 ) 组 成 ,连接 起 点 位 置 和 终点 位 置 的 序列 点 
或 曲线 称 为 路 径 , 构 成 路 径 的 策略 称 为 路 径 规 划 。 

路 径 规 划 是 运动 规划 的 主要 研究 内 容 之 一 。 路 径 是 机 器 人 位 姿 的 序列 ,而 不 考虑 机 器 人 
位 姿 参 数 随时 间 变 化 的 因素 ; 路 径 点 是 空间 中 的 位 置 或 关节 角度 。 路 径 规划 (一 般 指 位 置 规 
划 ) 是 找到 一 系列 要 经 过 的 路 径 点 ,而 轨迹 规划 是 赋予 路 径 时 间 信 息 ,路 径 规划 是 轨迹 规划 的 
基础 。 

运动 规划 ,又 称 运动 插 补 ,是 在 给 定 的 路 径 端 点 之 间 插 入 用 于 控制 的 中 间 点 序列 ,从 而 实 
现 沿 给 定 路 线 的 平稳 运动 。 运 动 控制 则 主要 解决 如 何 控制 目标 系统 准确 跟踪 指令 轨迹 的 问 
题 , 即 对 于 给 定 的 指令 轨迹 ,选择 适合 的 控制 算法 和 参数 ,产生 输出 ,控制 目标 以 实时 、 准 确 地 
跟踪 给 定 的 指令 轨迹 。 
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轨迹 规划 在 路 径 规划 的 基础 上 加 入 时 间 序 列 信息 ,对 机 器 人 执行 任务 时 的 速度 与 加 速度 
进行 规划 ,以 满足 光滑 性 和 速度 可 控 性 等 要 求 。 

路 径 规划 的 目标 是 使 路 径 与 障碍 物 的 距离 尽量 远 ,同时 路 径 的 长 度 尽量 短 。 而 轨迹 规划 
的 主要 目的 是 在 机 器 人 关节 空间 移动 时 使 得 机 器 人 的 运行 时 间 尽 可 能 短 , 或 者 所 消耗 的 能 量 
尽 可 能 小 。 

5.1.2 轨迹 规划 的 一 般 性 问题 

通常 将 机 械 臂 的 运动 看 作 是 工具 坐标 系 { 工 } 相 对 于 工件 坐标 系 {S } 的 一 系列 运动 。 这 种 
描述 方法 既 适 用 于 各 种 操作 臂 , 也 适用 于 同一 操作 臂 上 装 夹 的 各 种 工具 。 在 轨迹 规划 中 ,为 叙 
述 方便 ,也 常用 点 来 表示 机 器 人 的 状态 ,或 用 它 来 表示 工具 坐标 系 的 位 姿 ,例如 起 始点 、 终 止 点 
就 分 别 表示 工具 坐标 系 的 起 始 位 姿 及 终止 位 姿 。 

对 于 点 位 作业 的 机 器 人 (如 用 于 上 、 下 料 ), 需 要 描述 它 的 起 始 状 态 和 目标 状态 ,这 类 运动 
称 为 点 到 点 运动 。 而 对 于 曲面 加 工 类 作业 ,不 仅 要 规定 操作 臂 的 起 始点 和 终止 点 ,而 且 要 指明 
两 点 之 间 的 若干 中 间 点 ( 即 路 径 点 ) 必 须 沿 特定 的 路 径 运 动 (路 径 约 束 )。 这 类 运动 称 为 连续 路 
径 运 动 或 轮廓 运动 。 

在 对 机 器 人 的 运动 轨迹 进行 规划 时 ,往往 还 需要 知道 在 机 器 人 运动 的 路 径 上 是 否 存在 障 
碍 物 (障碍 约束 )。 路 径 约 束 和 障碍 约束 的 组 合 将 机 器 人 的 规划 与 控制 方式 划分 为 4 类 ,如 
表 5.1 所 示 。 存 在 障碍 物 的 情况 不 在 本 章 的 讨论 范围 内 ,本 章 主 要 讨论 连续 运动 且 无 障碍 的 
轨迹 规划 。 


表 5.1 机 器 人 的 规划 与 控制 方式 


离线 无 碰撞 路 径 规则 十 在 线路 径 跟 踪 
位 置 控制 十 在 线 障碍 探测 和 各 障 


离线 路 径 规划 十 在 线路 径 跟踪 


抱 全 全家 位 置 控制 


在 各 种 约束 条 件 下 ,期 望 机 械 臂 的 轨迹 在 整个 路 径 的 运动 过 程 都 是 平稳 而 光滑 的 。 因 此 ， 
应 该 为 路 径 规划 一 个 连续 的 且 具 有 一 阶 导数 的 平滑 函数 。 同 时 ,由 于 机 械 辟 特定 的 结构 等 各 
种 因素 ,机 械 臂 不 能 发 生 急速 运动 ,否则 将 加 剧 机 械 臂 各 结构 之 间 的 磨损 ,由 此 ,应 该 在 各 中 间 
点 之 间 , 对 路 径 的 空间 和 时 间 设 置 一 些 限制 条 件 。 

在 满足 上 面 各 种 条 件 的 前 提 下 ,轨迹 规划 有 两 种 方法 ,分 别 是 关节 空间 规划 方法 和 笛 卡 儿 
空间 规划 方法 。 也 就 是 说 ,轨迹 规划 既 可 在 关节 空间 中 描述 ,也 可 在 直角 坐标 空间 中 指定 ,从 
而 形成 了 关节 空间 和 直角 坐标 空间 机 器 人 轨迹 的 规划 方法 。 不 管 是 在 关节 空间 还 是 直角 坐标 
空间 ,都 要 保证 所 规划 的 轨迹 函数 必须 连续 和 平滑 ,使 机 器 人 的 运动 平稳 。 因 为 不 平稳 的 运动 
将 加 剧 机 械 部 件 的 磨损 ,并 导致 机 器 人 的 振动 和 冲击 。 为 此 ,要 求 所 选择 的 运动 轨迹 描述 函数 
必须 连续 ,而 且 它 的 一 阶 导数 (速度 ) ,有 时 甚至 是 二 阶 导数 (加 速度 ) 也 应 该 连续 。 

对 于 机 器 人 的 轨迹 规划 ,需要 运用 中 间 点 ( 即 路 径 点 ) 来 描述 期 望 的 路 径 。 其 中 ,对 每 个 中 
间 点 求 出 每 个 关节 坐标 系 的 位 姿 ,然后 将 每 个 关节 作为 单独 的 函数 设置 期 望 路 径 的 方法 称 为 
关节 空间 规划 法 ; 而 根据 每 个 中 间 点 的 位 姿 直 接 设置 期 望 路 径 的 方法 称 为 笛 卡 儿 空 间 规 划 
方法 。 

在 关节 空间 进行 规划 时 ,是 将 关节 变量 表示 成 为 时 间 的 函数 ,并 规划 它 的 一 阶 和 二 阶 时 间 
导数 ; 在 直角 空间 进行 规划 是 指 将 末端 执行 器 位 姿 . 速 度 和 加 速度 表示 为 时 间 的 函数 。 而 相 
应 的 关节 位 移 、 速 度 和 加 速度 由 末端 执行 器 的 信息 导出 。 通 常 通过 运动 学 反 解 得 出 关节 位 移 ， 
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用 北 雅 可 比 求 出 关节 速度 ,用 北 雅 可 比 及 其 导数 求解 关节 加 速度 。 


5.1.3 轨迹 的 生成 方式 

运动 轨迹 的 描述 或 生成 有 以 下 几 种 方式 。 

(1) 示 教 -再 现 运 动 。 这 种 运动 由 人 手把手 示 教 机 器 人 ,定时 记录 各 关节 变量 ,得 到 沿路 
径 运 动 时 各 关节 的 位 移 时 间 函 数 q(t); 再 现时 , 按 内 存 中 记录 的 各 点 的 值 产生 序列 动作 。 

(2) 关节 空间 运动 。 这 种 运动 直接 在 关节 空间 里 进行 。 由 于 动力 学 参数 及 其 极限 值 直 接 
在 关节 空间 里 描述 ,所 以 用 这 种 方式 求 最 短 时 间 运 动 很 方便 。 

(3) 空间 直线 运动 。 这 是 一 种 直角 空间 里 的 运动 , 它 便于 描述 空间 操作 ,计算 量 小 ,适宜 

简单 的 作业 。 

: (4) 空间 曲线 运动 。 这 是 一 种 在 描述 空间 中 用 明确 的 函数 表达 的 运动 ,如 圆周 运动 ,螺旋 
运动 等 。 


5.2 关节 空间 的 轨迹 规划 


随 着 机 器 人 使 用 得 越 来 越 广 ,对 机 器 人 的 轨迹 规划 的 要 求 也 越 来 越 高 ,机 器 人 轨迹 规划 是 
机 器 人 执行 作业 任务 的 基础 。 使 用 的 轨迹 规划 算法 ,直接 决定 了 机 器 人 的 运动 轨迹 .平滑 程 
度 , 使 点 位 运动 有 足够 高 的 精度 ,并 且 满 足 规划 路 径 的 约束 条 件 。 本 节 将 讨论 用 关节 角 的 函数 
描述 轨迹 的 生成 方法 。 

机 械 辟 运动 路 径 点 通常 是 用 工具 坐标 系 {T} 相 对 于 工作 台 坐 标 系 {S) 的 期 望 位 姿 来 确定 。 
对 于 关节 空间 规划 法 ,首先 确定 期 望 路 径 的 各 路 径 点 上 一 一 即 运 动 过 程 中 各 关节 的 坐标 系 上 
的 位 姿 ,然后 运用 逆 运 动 学 理论 ,将 路 径 点 变换 成 一 组 期 望 的 关节 角 。 通 过 这 样 变换 得 到 每 个 
路 径 点 处 各 关节 上 的 光滑 函数 。 由 于 上 述 过 程 是 建立 在 一 个 机 械 辟 期 望 位 姿 的 基础 上 ,所 以 
各 关节 到 达 各 期 望 路 径 点 位 置 所 用 的 时 间 都 是 相同 的 。 需 要 特别 说 明 的 是 ,虽然 每 个 关节 在 
同一 段 路 径 中 的 运动 时 间 相同 ,但 各 个 关节 的 轨迹 函数 是 相对 独立 的 ,其 期 望 的 关节 角 函 数 和 
其 他 关节 函数 无 关 。 | 

因此 ,用 关节 空间 规划 的 方法 对 机 器 人 进行 轨迹 规划 可 以 得 到 各 路 径 点 的 期 望 位 姿 。 总 
之 ,关节 空间 法 是 以 关节 角度 的 函数 来 描述 机 器 人 的 轨迹 ,关节 空间 法 不 必 在 直角 坐标 系 中 描 
述 两 个 路 径 点 之 间 的 路 径 形状 ,计算 比较 简单 。 并 且 由 于 关节 空间 与 直角 坐标 空间 之 间 不 是 
连续 的 对 应 关系 ,因而 不 会 发 生机 构 的 奇异 性 问题 。 

给 出 各 个 路 径 点 后 ,轨迹 规划 的 任务 包括 解 变换 方程 .进行 运动 学 反 解 和 插值 计算 。 在 关 
节 空 间 进行 规划 时 , 需 进 行 的 大 量 工作 是 对 关节 变量 的 插值 计算 。 对 关节 进行 插值 时 ,应 满足 
一 系列 约束 条 件 。 在 满足 所 有 约束 条 件 下 ,可 以 选取 不 同类 型 的 关节 插值 函数 ,生成 不 同 的 轨 
迹 。 关 节 轨 迹 的 插值 方法 较 多 ,如 三 次 多 项 式 插值 高 阶 多 项 式 插值 .用 抛物 线 过 渡 的 线性 插 
值 等 。 本 节 将 着 重 讨论 这 些 关节 轨迹 的 插值 方法 。 

5.2.1 三 次 多 项 式 插 值 

接 下 来 考虑 在 一 定时 间 内 将 工具 从 初始 位 置 移 动 到 目标 位 置 的 问题 。 由 于 机 械 臂 的 初始 
位 置 ( 即 机 械 臂 对 应 于 起 始点 的 关节 角度 0,) 已 知 ,可 以 应 用 逆 运 动 学 解 出 对 应 于 目标 位 置 的 
关节 角 9， 。 因 此 ,可 用 初始 位 置 关 节 和 角度 与 目标 位 置 关节 角度 的 一 个 平滑 插值 函数 0(z) 来 描 
述 运动 轨迹 。0(z) 在 io 时 刻 的 值 为 该 关节 的 初始 位 置 9, ,在 1， 时 刻 的 值 为 该 关节 的 目标 位 
置 9; 。 有 多 种 光滑 函数 可 作为 关节 插值 函数 ,如 图 5.1 所 示 。 

为 实现 单个 关节 的 平稳 运动 ,显然 轨迹 函数 0(1) 需 要 满足 4 个 约束 条 件 , 即 两 端点 位 置 
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约束 和 两 端点 速度 约束 。 通 过 选择 初始 位 置 和 目标 位 置 可 
得 到 对 轨迹 函数 9(z) 的 两 个 约束 条 件 : 


0(0) =0, 
| (5. 1) 
Op) 一 07 Rt We 
另外 ,两 个 约束 条 件 需 要 保证 关节 运动 速度 函数 的 连 O Pa 
续 性 , 即 在 初始 位 置 和 目标 位 置 的 关节 速度 要 求 。 一 般 情 图 5.1 某 一 关节 不 同 的 轨迹 曲线 
况 下 设 为 零 , 即 
0(0) =0 
| (5. 2) 
OAGtr 7) 一 0 
式 (5. 1) 和 式 (5. 2) 唯 一 确定 了 一 个 三 次 多 项 式 , 形 式 如 下 ; 
b(G) 一 ao 十 ait 十 azt2 十 ast3 (5. 3) 
则 对 应 于 该 运动 轨迹 的 关节 速度 和 加 速度 分 别 为 
0(t) =ail+2ast + 3at’ 
(5. 4) 
6(1) =24, + 6ast 


把 上 述 约 束 条 件 式 (5. 1) 和 式 (5. 2) 代 入 式 (5.3) 和 式 (5. 4) ,可 得 含有 4 个 系数 4a。、al、a, 和 
ai 的 线性 方程 : 


00 一 al 


07 一 0 十 altr 十 aztz 十 ast 


( 545 
0 一 al 
0=al 十 2a2tr 二 3a3t7 
解 式 (5.5) 可 得 
ao 一 00 
Q1 一 从 
3 
tf 


Q3 re 
te 


应 用 式 (5.6) 可 以 求 出 从 任意 初始 关节 角 位 置 到 目标 (终止 ) 位 置 的 三 次 多 项 式 , 但 该 组 解 - 
只 适用 于 起 始 关 节 速 度 和 终止 关节 速度 为 零 的 情况 。 

例 5.1 设 机械 臂 某 个 关节 的 起 始 关节 和 角 0,=15", 并 且 机 械 手 原来 是 静止 的 ,要 求 它 在 
3s 内 平滑 地 运动 到 9, 二 75” 时 停 下 来 ( 即 要 求 在 终端 时 的 速度 为 零 )。 请 规划 出 满足 上 述 条 
件 的 平滑 运动 的 轨迹 ,并 画 出 关节 角 位 置 .角速度 及 角 加 速度 随时 间 变 化 的 曲线 。 

解 : 根据 要 求 ,可 以 对 该 关节 采用 三 次 多 项 式 插值 函数 来 规划 其 运动 。 已 知 0, 二 15°. 
0 二 75" ,tp 二 3s ,代入 式 (5.6) 可 得 三 次 多 项 式 的 系数 a 二 15,a1 二 0,4as 二 20,as 二 一 4.44。 

由 式 (5. 3) 和 式 (5. 4) 可 确定 该 关节 的 运动 轨迹 , 即 

人 一 15 十 2042 一 4.4413 


0(t) =40t 一 13. 33° (5.7) 
6(1) =40— 26. 661 
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根据 式 (5.7) 画 出 它们 随时 间 的 变化 曲线 如 图 5. 2 所 示 。 由 图 可 以 看 出 ,速度 册 线 为 一 抛 


物 线 ,加 速度 则 为 一 直线 。 


0 
4 


to 
O 


Wa yt 
(a) 角 位 移 (b) 角速度 (c) 角 加 速度 
. 图 5.2 ”机 械 臂 的 某 个 关节 的 运动 轨迹 曲线 


5 本 汉 过 路 径 点 的 三 次 多 项 式 插值 
一 般 而 言 ,希望 规划 过 路 径 点 的 轨迹 。 如 图 5. 3 所 示 , 机 器 人 作业 除 在 A、B 点 有 位 姿 要 


求 外 ,在 路 径 点 C、D 也 有 位 姿 要 求 。 对 于 这 种 情况 ,假如 末端 B 

执行 器 在 路 径 点 停留 , 即 各 路 径 点 上 速度 为 0, 则 轨迹 规划 可 连 

续 直 接 使 用 前 面 介绍 的 三 次 多 项 式 插值 方法 。 通 常情 况 下 , 霖 C 
端 执行 器 只 是 连续 经 过 每 个 路 径 点 ,并 不 停留 ,就 需要 将 前 述 方 

法 推广 。 


D 


- 径 点 
实际 上 ,可 以 把 所 有 路 径 点 也 看 作 是 “起 始点 ”或 “终止 点 ”， 图 5.3 机 器 人 作业 路 径 


通过 逆 运 动 学 求解 每 个 路 径 点 对 应 的 期 望 关 节 角 。 然 后 ,对 每 个 关节 求 出 能 平滑 地 经 过 每 个 
路 径 点 的 三 次 多 项 式 插值 函数 。 但 是 ,这 些 “ 起 始点 ”和 "终止 点 ”处 的 关节 运动 速度 不 再 是 零 。 


如 果 已 知 各 关节 在 路 径 点 的 期 望 速度 , 则 可 用 前 面 所 述 方法 确定 三 次 多 项 式 。 不 同 的 是 ， 


每 个 终止 点 处 的 速度 约束 条 件 不 再 为 零 , 式 (5.2) 的 约束 条 件 变 为 


呈 =0, 


《5. 8) 
三 次 多 项 式 的 4 个 方程 为 
bo 一 ao 
0r 一 ao 十 aitr 十 azt? 十 ast》 
: 《S59 
bo 一 41 
Os =a] 十 2aztfp 十 3a3t7 
求解 方程 组 (5. 9) ,可 得 三 次 多 项 式 的 系数 为 
40 一 《0 
4&5 =0, 尼 
3 2 ， Ys 
WE (5. 10) 
2 天 
as 一 0 -= 原 Y 填 福 > (Op 十 0o) 


式 (5. 1 可 训 示 二 中 看 在 二 从 定 记 王 和 过 证 训 拓 夫 人 大 交 放 长 调 次 多 项 式 。 若 每 个 路 
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径 点 处 都 有 期 望 的 关节 速度 ,可 以 使 用 以 下 几 种 方法 来 确定 路 径 点 处 的 关节 速度 。 

方法 (1): 根据 工具 坐标 系 的 直角 坐标 空间 中 的 瞬时 线 速度 和 角速度 确定 每 个 路 径 点 的 

瞬时 期 望 关 节 速 度 ,该 方法 的 工作 量 较 大 。 
对 于 方法 (1) ,利用 操作 臂 在 此 路 径 点 上 的 道 雅 可 比 ,把 该 点 的 直角 坐标 速度 “映射 "为 所 
要 求 的 关节 速度 。 当然 ,如 果 操 作 臂 的 某 个 路 径 点 是 奇异 点 ,这 时 就 不 能 任意 设置 速度 值 。 按 
照 方法 (1) 生 成 的 轨迹 虽然 能 满足 用 户 设置 速度 的 需要 ,但 是 逐 点 设置 速度 毕竟 要 耗费 很 大 
的 工作 量 。 因 此 ,机 器 人 的 控制 系统 最 好 具有 方法 (2) 或 方法 (3) 的 功能 ,或 者 二 者 兼 而 
有 之 。 

方法 (2) : 在 直角 坐标 空间 或 关节 空间 使 用 适当 的 启发 式 方法 ,由 控制 系统 自动 选取 路 径 
点 的 速度 。 

对 于 方法 (2) 系 统 采用 某 种 启发 式 方法 自动 选取 合适 的 路 径 点 速度 。 启 发 式 选 择 路 径 点 
速度 的 方式 如 图 5.4 所 示 。 图 5.4 中 0 为 起 始点 ,0p 为 终 
止 点 ,04 .9p 和 0c 是 路 径 点 ,用 细 实 线 表示 过 路 径 点 时 的 关 
节 运 动 速度 。 这 里 所 用 的 启发 式 信 息 从 概念 到 计算 方法 都 很 
简单 一 一 假设 用 虚线 段 把 这 些 路 径 点 依次 连接 起 来 ,如 果 相 
邻 线段 的 斜率 在 路 径 点 处 改变 符号 , 则 把 速度 选 定 为 零 ; 如 

wd tc tp t+ 果 相 邻 线段 不 改变 符号 , 则 选取 路 径 点 两 侧 的 线段 斜率 的 平 
图 5.4 路 径 点 上 速度 的 自动 生成 均值 作为 该 点 的 速度 。 因 此 ,根据 规定 的 路 径 点 ,系统 就 能 够 
按 此 规则 自动 生成 相应 的 路 径 点 速度 。 
方法 (3) : 采用 使 路 径 点 处 的 加 速度 连续 的 方法 ,由 控制 系统 按 要 求 自 动 选取 路 径 点 的 
速度 。 
对 于 方法 (3) ,为 了 保证 路 径 点 处 的 加 速度 连续 ,可 以 设法 用 两 条 三 次 曲线 在 路 径 点 处 按 
一 定 规则 连接 起 来 ,拼凑 成 所 要 求 的 轨迹 。 其 约束 条 件 是 : 连接 处 不 仅 速 度 连续 ,而 且 加 速度 
也 连续 (本 节 将 不 对 此 作 具 体 介 绍 ) 。 


5.2.3 高 阶 多 项 式 插 值 
在 前 述 轨迹 插值 的 基础 上 , 若 对 运动 轨迹 的 要 求 增 加 ,如 确定 路 径 段 的 起 始点 和 终止 点 的 


位 置 .速度 和 加 速度 , 则 三 次 多 项 式 就 无 法 满足 需要 。 此 时 ,需要 用 更 高 阶 的 多 项 式 五 次 
多 项 式 对 运动 轨迹 的 路 径 进 行 插值 , 即 

0(t) =aoTait+ast :ast +at! t+ast’ 《区 

相应 地 ,需要 在 前 述 基础 上 增加 2 个 对 加 速度 的 约束 条 件 ,多 项 式 需要 满足 以 下 6 个 约束 _ 

条 件 : 

00 =ao 

94 一 ao 十 aitr 十 az 弛 十 as 好 十 ad 弛 十 astr 

bu un 

A (5: 12) 

0 一 Qi 十 2astr 十 3asty | dar dat 

ba 


by 一 2ay 十 6astr 十 12z4ty + 20asty 
方程 组 (5. 12) 有 6 个 未 知 数 和 6 个 方程 ,求解 可 得 
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4a0 一 00 
Ql 一 如 

四 
ds 一 名 
,S007 2000— C80 F-12002 — C3 —D dz 


Ls (5. 13) 
- DE 
300。 — 300 4 一 (1407 +1600)t, — (36, — 20,)3 
Fe Wa 
£ 
120/ —1200 一 (60/ + 600)t, — (0 一 六 ) 弛 
We 


5 

5.2.4 用 抛物 线 过 渡 的 线性 插值 

在 关节 空间 轨迹 规划 中 ,对 于 给 定 起 始点 和 终止 点 的 情况 ,选择 线性 函数 插值 较为 简单 ， 
如 图 5.5 所 示 。 值 得 注意 的 是 ,该 方法 中 虽然 各 关节 的 运动 是 线性 
的 ,但 末端 执行 器 的 运动 轨迹 一 般 不 是 线性 的 。 

此 外 ,线性 插值 将 会 导致 起 始点 和 终止 点 处 关节 运动 的 速度 不 
连续 ,加 速度 无 限 大 , 即 在 两 个 端点 会 造成 刚性 冲击 。 为 此 , 需 对 线 
性 函数 插值 方案 进行 修正 一 一 在 线性 插值 两 端点 的 邻 域 内 设置 一 段 
抛物 线形 缓冲 区 段 ,从 而 使 整个 轨迹 上 的 位 移 和 速度 都 连续 。 由 于 wt 
抛物 线 函 数 对 于 时 间 的 二 阶 导 数 为 常数 , 即 相 应 区 段 内 的 加 速度 恒 5.5 线性 函数 插值 
定 , 以 保证 起 始点 和 终止 点 的 速度 平滑 过 渡 , 从 而 使 整个 轨迹 上 的 位 
置 和 速度 连续 。 线 性 函数 与 两 段 抛物 线 函 数 平滑 地 衔接 在 一 起 形成 的 轨迹 称 为 带 有 抛物 线 过 
渡 域 的 线性 轨迹 。 

在 运动 轨迹 的 拟 合 区 段 内 ,使 用 恒定 加 速度 平滑 地 改变 速度 。 使 用 这 种 方法 构造 的 简单 
路 径 如 图 5.6 所 示 。 为 了 构造 这 样 的 路 径 段 ,假设 两 端的 抛物 线 拟 合 区 段 具 有 相同 的 持续 时 
间 上 ,具有 大 小 相同 而 方向 相反 的 恒 加 速度 06。 对 于 这 种 路 径 规划 存在 有 多 个 解 ,其 轨迹 不 唯 
一 ,如 图 5.7 所 示 。 但 是 ,每 条 路 径 都 对 称 于 时 间 中 点 如 和 位 置 中 点 0 。 要 保证 路 径 轨 迹 的 
连续 ,光滑 , 即 要 求 抛物 线 轨 迹 的 终点 速度 必须 等 于 线性 段 的 速度 , 故 有 下 列 关系 

0, 二 多 
i 一 站 a 
式 中 , 9。 为 对 应 于 抛物 线 持续 时 间 t， 的 关节 角度 ,6 表示 拟 合 区 段 的 加 速度 。0。 的 值 可 以 由 
下 式 求 出 : 


6 (5. 14) 


9 二 0 十 广 六 ? (5. 15) 
设 关 节 从 起 始点 到 终止 点 的 总 运动 时 间 为 1 , 则 zj 二 2 ,并 注意 到 
、 1 


则 由 式 (5. 14) 一 式 (5. 16) 得 
Wt 二 (04 一 00) 一 0 (5.17) 
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一 般 情况 下 ,bu .07 .zy 是 已 知 条件 , 这 样 , 据 式 (5.14) 可 以 选择 相应 的 和 +, ,得 到 相应 
的 轨迹 。 通 常 的 做 法 是 先 选 定 加 速度 的 值 ,然后 按 式 (5.17) 求 出 相应 的 1: 


纪 一 了 (5. 18) 
由 式 (5. 18) 可 知 ,为 保证 上 +。 有 解 ,加 速度 值 6 必须 选 得 足够 大 , 即 

，、4(9/ 一 00) 

一 一 一 一 (5. 19) 


ti 

当 式 (5.19) 中 的 等 号 成 立时 ,轨迹 线性 段 的 长 度 缩 减 为 零 , 整 个 轨迹 由 两 个 过 渡 域 组 成 ， 
这 两 个 过 渡 域 在 衔接 处 的 斜率 (关节 速度 ) 相 等 。 加 速度 6 的 取 值 越 大 ,过 渡 域 的 长 度 会 变 得 
越 短 ; 若 加 速度 趋 于 无 穷 大 ,轨迹 又 恢复 到 简单 的 线性 插值 情况 。 


0 
Or 0 
0/-0a Or 
0, 0 
Oo Oo 
ng yy 1 0 0 
图 5.6 带 有 抛物 线 过 渡 域 的 线性 轨迹 图 5.7 轨迹 的 多 解 性 与 对 称 性 


例 5.2 同 例 5.1, 已 知 0 一 15",0/ 一 75",tr 一 3s, 试 设计 两 条 带 有 抛物 线 过 渡 的 线性 轨迹 。 
解 : 根据 题 意 , 按 式 (5.19) 定 出 加 速度 的 取 值 范围 ,为 此 ,将 已 知 条 件 代 入 式 (5. 19) 中 ,有 
6>26. 67°/s’。 
(1) 设计 第 一 条 轨迹 。 
对 于 第 一 条 轨迹 ,如 果 选 = 二 42"/s’ ,由 式 (5.15) 算 出 过 渡 时 间 二 , 则 
8 AD dW CRE SY. 


= 一 一 一 一 一 0. * 
Pod 9 2 X42 ey 


用 式 (5. 15) 和 式 (5. 14) 计 算 过 渡 域 结束 时 的 关节 位 置 0 和 关节 速度 9 ,得 
区 二 二 ( WX 0 59! | =22, 3 


0 =0#,1 =(42 xX 0, 59)°/s =24:78°/s 
根据 上 面 计算 得 出 的 数值 可 以 绘 出 如 图 5.8(a) 所 示 的 轨迹 曲线 。 
(2) 设计 第 二 条 轨迹 。 

对 于 第 二 条 轨迹 , 若 选择 6, 二 27"/s? ,可 求 出 
3 N27 XI — dMLITS 15) 


fos = 二 一 1,38 
2 2 


0 ， =15+ (了 X27x = 


0, =6,t,, =(27X1.33)°/s=35.91°/s 
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相应 的 轨迹 曲线 如 图 5. 8(b) 所 示 。 


.总 ;过 
过 说 
坝 其 
下 亚 
06 12 1.8 2.4 3.07 0 一 06 18 24 3.07 
位 移 时 间 w 速度 时 间 ws 加 速度 时 间 w/s 
(a) 加 速度 较 大 时 的 位 移 、 速 度 、 加 速度 曲线 
0 6 
和 42 
a _ 36 
20 加 24 
.总 15 Es 人 
入 人 的 0.6 1.2 1.8 
4 | 
| -36 
Dy em i 24 0 dol or no le 
位 移 时 间 w/s 速度 时 间 w/s 加 速度 时 间 ws 


(b) 加 速度 较 小 时 的 位 移 、 速 度 、 加 速度 曲线 
图 5.8 带 有 抛物 线 过 滤 的 线性 插值 


用 抛物 线 过 渡 的 线性 函数 插值 进行 轨迹 规划 的 物理 概念 非常 清楚 , 即 如 果 机 器 人 每 一 关 
节 电 动机 采用 等 加 速 ,等 速 和 等 减速 运动 规律 , 则 关节 的 位 置 、 速 度 、 加 速度 随时 间 变 化 的 曲线 
如 图 5. 8 所 示 。 

若 某 个 关节 的 运动 要 经 过 一 个 路 径 点 , 则 可 采用 带 抛物 线 过 渡 域 的 线性 路 径 方 案 。 关 节 
的 运动 要 经 过 一 组 路 径 点 ,如 图 5. 9 所 示 ,用 关节 4 
角度 9; .9 和 09, 表示 其 中 3 个 相 邻 的 路 径 点 ,以 
线性 函数 将 每 两 个 相 邻 路 径 点 之 间 相 连 , 而 所 有 
路 径 点 附近 都 采用 抛物 线 过 渡 。 

应 该 注意 到 : 各 路 径 段 采用 抛物 线 过 渡 域 线 
性 函数 所 进行 的 规划 ,机 器 人 的 运动 关节 并 不 能 
真正 到 达 那 些 路 径 点 。 即 使 选取 的 加 速度 足够 
大 ,实际 路 径 也 只 是 十 分 接近 理想 路 径 点 ,如 el | 
图 5.9 所 示 。 图 5.9 多 段 带 有 抛物 线 过 渡 域 的 线性 轨迹 

关节 空间 规划 法 表明 ,从 关节 空间 得 出 的 各 
中 间 点 对 应 的 各 关节 的 关节 角 函 数 可 以 保证 机 械 臂 到 达 目 标的 位 置 。 不 过 ,由 于 关注 的 是 关 
节 的 变化 ,所 以 对 于 机 械 臂 的 末端 执行 器 ,其 在 空间 中 的 路 径 并 不 确定 ,而 且 因为 要 用 到 道 运 
动 学 公式 ,所 以 路 径 的 复杂 性 取决 于 各 机 器 人 特定 的 运动 学 特性 。 相 比 之 下 , 笛 卡 儿 空 间 规划 
方法 是 用 每 个 中 间 点 的 位 姿 ( 注 : 它 是 关于 时 间 的 函数 ) 来 规划 路 径 ,这 种 方法 可 以 确定 各 中 
间 点 之 间 的 具体 形状 ,如 最 常见 的 直线 或 是 正弦 、 抛 物 线 甚 至 是 圆 。 不 过 这 种 运算 量 会 很 大 ， 
因为 运行 时 在 空间 中 生成 相关 路 径 的 时 候 , 必 须 通 过 道 运 动 学 实时 地 解 出 对 应 的 关节 角 。 
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5.3 直角 坐标 空间 的 轨迹 规划 


给 出 各 个 路 径 结 点 后 ,轨迹 规划 的 任务 包括 解 变换 方程 .进行 运动 学 反 解 和 插值 计算 。 在 
关节 空间 进行 规划 时 ,可 以 保证 运动 轨迹 经 过 给 定 的 路 径 点 , 需 进 行 的 大 量 工作 是 对 关节 变量 
的 插值 计算 。 关 节 空 间 规划 法 表明 ,从 关节 空间 得 出 的 各 中 间 点 对 应 的 各 关节 的 关节 角 函 数 
可 以 保证 机 械 臂 到 达 目 标的 位 置 。 不 过 ,由 于 关注 的 是 关节 的 变化 ,所 以 对 于 机 械 臂 的 末端 执 
行 器 ,其 在 空间 中 的 路 径 并 不 确定 ,而且 因 为 要 用 到 递 运动 学 公式 ,所 以 路 径 的 复杂 性 取决 于 
各 机 器 人 特定 的 运动 学 特性 。 

相 比 之 下 , 笛 卡 儿 空间 规划 方法 (直角 坐标 空间 ) 是 用 每 个 中 间 点 的 位 姿 关 于 时 间 的 函数 
来 规划 路 径 , 这 种 方法 可 以 确定 各 中 间 点 之 间 的 具体 形状 ,如 最 常见 的 直线 .正弦 曲线 .抛物线 
甚至 是 圆 。 但 是 在 直角 坐标 空间 中 ,路 径 点 之 间 的 轨迹 形状 往往 是 十 分 复杂 的 , 它 取 决 于 机 械 
臂 未 端 执行 器 的 运动 学 机 构 特 性 。 这 将 导致 该 方法 的 运算 量变 得 很 大 ,因为 运行 时 在 空间 中 
生成 相关 路 径 的 时 候 , 必 须 实时 通过 逆 运 动 学 解 出 对 应 的 关节 角 。 在 有 些 情况 下 ,对 机 械 臂 末 
端的 轨迹 形状 也 有 一 定 要 求 , 如 要 求 它 在 两 点 之 间 走 一 条 直线 ,或 沿 着 一 个 圆 弧 运动 以 绕 过 障 
碍 物 等 。 这 时 便 需要 在 直角 坐标 空间 内 规划 机 械 辟 的 运动 轨迹 。 

直角 坐标 空间 的 路 径 点 , 指 的 是 机 械 臂 示 端的 工具 坐标 相对 于 基 坐 标的 位 置 和 姿态 。 每 
一 个 点 由 6 个 量 组 成 ,其 中 3 个 量 用 来 描述 位 置 ,另外 3 个 量 用 来 描述 姿态 。 在 直角 坐标 空间 
内 ,规划 的 方法 主要 有 线性 函数 插 补 ( 值 ) 法 和 圆 弧 插 补 ( 值 ) 法 。 

直线 插 补 (线性 函数 择 补 ) 和 圆 弧 插 补 是 机 器 人 系统 中 的 基本 插 补 算法 。 对 于 非 直线 和 圆 
弧 轨 迹 ,可 用 直线 或 圆 弧 副 近 , 以 实现 这 些 轨 迹 。 


5.3.1 直线 插 补 
空间 直线 插 补 是 在 已 知 该 直线 始末 两 点 的 位 置 和 姿态 的 条 件 下 , 求 各 轨迹 中 间 点 ( 插 补 
点 ) 的 位 置 和 姿态 。 由 于 在 大 多 数 情 况 下 ;机 器 人 沿 直线 运动 A 


时 ,其 姿态 不 变 , 所 以 无 姿态 插 补 , 即 保持 第 一 个 示 教 点 时 的 姿 
态 。 当 然 ,在 有 些 情况 下 要 求 变 化 姿态 ,这 就 需要 姿态 插 补 ,可 
仿照 下 面 介绍 的 位 置 插 补 原理 来 处 理 , 也 可 参照 圆 弧 的 姿态 插 PC Yo, 20) 
补 方法 解决 ,如 图 5. 10 所 示 。 已 知 直线 始末 两 点 的 坐标 值 P。 
(KooY0sZ0) 和 卫 ,( 久 ,sj2,》 以 及 其 姿态 其 中 PosP, 嘎 
相对 于 基 坐 标 系 的 位 置 。 这 些 已 知 的 位 置 和 姿态 通常 是 通过 入 
示 教 方式 得 到 的 。 设 " 为 沿 直线 运动 的 期 望 速度 ,t, 为 插 补 时 


图 5.10 空间 直线 插 补 


间 间 隔 。 
为 减少 实时 计算 量 , 示 教 完 成 后 ,可 求 出 直线 长 度 为 : 
L=/(¥, —X0) +(Y,—Y) +(2,—2Z0) (5. 20) 
i, 间隔 内 行程 为 4 二 vt, ; 插 补 总 步 数 N 为 L/d 十 1 的 整数 部 分 ; 各 轴 增 量 为 
人 一 发 7 
AX = 
di ， (5.21) 
N 
(2Z, 一 Zu) 
a 


第 5 章 ”机 器 人 轨迹 规划 | 加 57 


各 插 补 点 坐标 值 为 
Xi =X, 十 iAX 
=Y, +iAY (5. 22) 
芒 一 Z, 十 iAZ 
式 中 ,i=0,1,2,…,N。 


5.3.2 . 圆 泊 插 补 


1. 平面 圆 弧 插 补 
平面 圆 弧 是 指 圆 弧 平 面 与 基 坐 标 系 的 三 大 平面 之 一 重合 ,以 XOY 平面 圆 弧 为 例 。 已 知 
不 在 一 条 直线 上 的 三 点 P .P,、P, 及 这 三 点 对 应 的 机 器 人 手 端的 姿态 ,如 图 5. 11 和 图 5. 12 
所 示 。 : | 
Pa 万) 
其 
Pin(Xin, Fin) 
R 
P(X, Y») 
a 7 PX, ¥) 
hh Bi 二 = 
图 5.11 由 已 知 点 Pl、P,、P; 决定 的 圆 弧 图 5.12 圆 弧 插 补 


设 v 为 沿 圆 弧 运动 速度 ; t, 为 插 补 时 时 间隔 。 类 似 于 直线 插 补 情况 计算 出 : 
(1) 由 Pl、P,、P, 决定 的 圆 弧 半 径 RR。 
(2) 总 的 圆心 角 $= 二 $1 十 $s, 即 
$1 一 arccos {([(X, — X1): 十 (Y —Y)’—2R°]/2R’} 
. (5. 23) 
$2 一 arccos {[(Xs —X,) (Ys —Y,)’ —2R°]/2R’)} 


(3) t, 时 间 内 角 位 移 量 为 A0==t,v/R , 据 图 5. 10 所 示 的 几何 关系 求 各 插 补 点 坐标 。 
(4) 总 插 补 步 数 ( 取 整 数 ) 为 
N=$/A0+1 
对 P,+1 点 的 坐标 ,有 
X, 一 RRcos(0; 十 Ab) 一 尺 cosbi cosA0 一 人 sin0i sinA0 一 XicosA0 一 YisinAO0 
式 中 : X, 二 Reos0;; Y,; 王 Rsin0i。 
同 理 有 
Y，， =Rsin(0; 十 Ab) 一 Rsing,cosAb 一 Recosb,sinAg 一 YicosAb 一 和 ,sinAb 
由 0 一 0 十 Ab 可 判断 是 否 到 插 补 终点 。 若 0 1 三 #$, 则 继续 插 补 下 去 ; 当 0;+1 盖 风 时 ， 
则 修正 最 后 一 步 的 步 长 Ab ,并 以 Ab 表示 ,Ab 一 % 一 0; , 故 平面 圆 弧 位 置 插 补 为 
Cr =X,cosA0 — Y,sinA0 
Y;41 =Y;cosA0 — X,sinA0 (5 24) 
le, =0; 十 A0 
2. 空间 圆 弧 插 补 
空间 圆 弧 是 指 三 维 空间 任 一 平面 内 的 圆 弧 , 空 间 圆 弧 插 补 可 分 为 以 下 三 步 。 
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@ 把 三 维 问题 转化 成 二 维 , 找 出 圆 弧 所 在 Zo 
平面 。 Pi Yi, 21) 
@ 利用 二 维 平面 插 补 算法 求 出 插 补 点 坐标 Ps(&, Yo, D) 


P(Xs, Ys, 23) 


(Kit1sY;t1)e 

@ 把 该 点 的 坐标 值 转变 为 基础 坐标 系 下 的 
值 ,如 图 5. 13 所 示 。 

通过 不 在 同一 直线 上 的 三 个 点 Pi P,P 
可 确定 一 个 圆 及 三 点 间 的 圆 弧 ,其 圆心 为 Ok , 半 
径 为 尺 , 圆 弧 所 在 平面 与 基础 坐标 系 平面 的 交 线 x 
分 别 为 AB、BC、CA 。 建 立 圆 弧 平 面 插 补 坐标 
系 , 即 把 ORXRYRZR 坐标 系 原点 与 圆心 Ok 重 
合 , 设 ORXRYRZR 平面 为 圆 弧 所 在 平面 , 且 保 持 Zk 为 外 法 线 方向 。 这样, 一 个 三 维 问 题 就 
转化 成 平面 问题 ,可 以 应 用 平面 圆 弧 插 补 的 结论 。 

求解 两 坐标 系 ( 见 图 5. 13) 的 转换 矩阵 。 令 Tk 表示 由 圆 弧 坐标 OkRXRYRZR 至 基础 坐标 
系 OXoYoZo 的 转换 矩阵 。 若 Zk 轴 与 基础 坐标 系 Zo 轴 的 夹 角 为 a,Xk 轴 与 基础 坐标 系 的 
夹 角 为 9, 则 可 完成 下 述 步骤 : 

@ 将 XRYR2ZR 的 原点 Ok 放 到 基础 原点 O 上 ; 

四 绕 Zk 轴 转 0, 使 站。 与 Xk 平行 ; 

@ 再 绕 Xk 轴 转 a 角 , 使 Zu 与 Zk 平行 。 

这 三 步 完 成 了 XRYRZR 向 XuYuZ 的 转换 , 故 总 转换 矩阵 应 为 

T= RR 


图 5.13 基础 坐标 与 空间 圆 弧 平 面 的 关系 


cosO 一 Sin0cosO singcos0 XoR 
sing cosbcosa ”一 cosbgsina Yor 
二 G8. 25) 
0 sina cosa ZOR 
0 0 0 2 
式 中 ,XoR 、Yor 、Zor 为 圆心 OR 在 基础 坐标 系 下 的 坐标 值 。 
要 将 基础 坐标 系 的 坐标 值 表 示 在 ORXRYRZR 坐标 系 , 则 需要 用 到 TR 的 逆 和 矩阵 
cos0 sinb 0 — (Xorcos0 十 YoR sing) 
a 一 Sin0cosO cosbcosa sinag 一 (CXoRSsin0gcosa 十 YoRcosOcosa 十 ZoRsina) 
Pa singsinaw ”一 cosbOsina cosa — (Xorsindsina 十 YoRcosOsina + Zorcosa) 
0 0 0 | 
(5. 26) 


5.4 本 章 小 结 


本 章 基于 机 械 臂 运动 学 和 动力 学 基础 ,讨论 了 关节 空间 和 直角 坐标 空间 中 机 器 人 的 运动 
轨迹 规划 方法 。 先 对 轨迹 规划 的 基本 概念 和 一 般 问题 进行 了 简单 阐述 ; 然后 重点 讨论 了 关 市 
空间 轨迹 的 插值 方法 ,包括 三 次 多 项 式 插 值 .高 阶 多 项 式 插值 .用 抛物 线 过 渡 的 线性 插值 等 方 
法 ,并 针对 个 别 插值 方法 进行 举例 说 明 ; 最 后 讨论 了 两 种 直角 坐标 空间 轨迹 规划 的 方法 一 一 
直线 持 补 和 圆 弧 插 补 方法 ,其 中 国 弧 插 补 方法 又 包括 平面 圆 弧 插 补 和 空间 圆 弧 插 补 。 
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CHAPTER 6 


第 2 章 和 第 4 章 分 别 讨论 了 机 器 人 的 运动 学 建 模 和 动力 学 建 模 问题 ,第 5 章 重 点 研究 了 
机 器 人 的 轨迹 规划 方法 。 通 过 前 面 章节 的 知识 ,可 以 计算 关节 位 置 的 时 间 历 程 以 及 这 些 关 节 
位 置 对 应 于 末端 执行 器 通过 空间 的 期 望 运动 。 本 章 将 在 前 述 章节 的 基础 上 研究 如 何 使 机 械 臂 
完成 这 些 期 望 运动 , 即 机 器 人 的 控制 问题 。 本 章 将 重点 讨论 机 器 人 的 位 置 控制 力 控制 力 / 位 
置 混合 控制 ,后续 章节 将 详细 介绍 一 些 现 代 控 制 方法 (如 滑 模 变 结构 控制 . 自 适应 控制 等 ), 以 
及 模糊 控制 .学习 控制 等 智能 控制 方法 。 


6.1 机 器 人 控制 系统 概述 


6.1.1 机 器 人 控制 系统 的 特点 


控制 系统 是 决定 机 器 人 功能 和 性 能 的 主要 因素 ,在 一 定 程 度 上 制约 着 机 器 人 技术 的 发 展 ， 
它 的 主要 任务 就 是 控制 机 器 人 在 工作 空间 中 的 运动 位 置 .姿态 和 轨迹 、 操 作 顺 序 及 动作 的 时 间 
等 。 多 数 机 器 人 的 结构 是 一 个 空间 开 链 结构 ,各 关节 的 运动 是 相互 独立 的 ,为 了 实现 机 絮 人 末 
端 执行 器 的 运动 ,需要 多 关节 协调 运动 ,因此 ,与 普通 的 控制 系统 相 比 ,机 器 人 控制 系统 要 复杂 
一 些 。 具 体 来 讲 , 机 器 人 控制 系统 主要 具有 以 下 特点 。 

(1) 机 器 人 控制 系统 是 一 个 多 变量 控制 系统 ,即使 是 简单 的 工业 机 器 人 也 有 3 一 5 个 自由 
度 ,比较 复杂 的 机 器 人 有 十 几 个 自由 度 , 甚 至 几 十 个 自由 度 。 每 个 自由 度 一 般 包含 一 个 伺服 机 
构 ,多 个 独立 的 伺服 系统 必须 有 机 地 协调 起 来 。 例 如 ,机 器 人 的 手 部 运动 是 所 有 关节 的 合成 运 
动 ,要 使 手 部 按照 一 定 的 轨迹 运动 ,就 必须 控制 各 关节 协调 运动 ,包括 运动 轨迹 、 动 作 时 序 等 多 
方面 的 协调 。 

(2) 运动 描述 复杂 ,机 器 人 的 控制 与 机 构 运 动 学 及 动力 学 密切 相关 。 描 述 机 器 人 状态 和 
运动 的 数学 模型 是 一 个 非 线性 模型 , 随 着 状态 的 变化 ,其 参数 也 在 变化 ,各 变量 之 间 还 存在 耦 
合 。 因 此 ,仅仅 考虑 位 置 闭环 是 不 够 的 ,还 要 考虑 速度 闭环 ,甚至 加 速度 闭环 。 在 控制 过 程 中 ， 
根据 给 定 的 任务 ,应 当选 择 不 同 的 基准 坐标 系 ,并 做 适当 的 坐标 变换 ,求解 机 器 人 运动 学 正 问 
题 和 逆 问 题 。 此 外 ,还 要 考虑 各 关节 之 间 惯 性 力 ` 哥 氏 力 等 的 耦合 作用 和 重力 负载 的 影响 , 因 
此 ,系统 中 还 经 常 采 用 一 些 控 制 策 略 ,如 重力 补偿 、 前 馈 、 解 看 或 自 适应 控制 等 。 

(3) 具有 和 较 高 的 重复 定位 精度 ,系统 刚性 好 。 除 直角 坐标 机 器 人 外 ,机 器 人 关节 上 的 位 置 
检测 元 件 不 能 安装 在 末端 执行 器 上 ,而 应 安装 在 各 自 的 驱动 轴 上 ,构成 位 置 半 闭 环 系统 。 但 机 
器 人 的 重复 定位 精度 较 高 ,一般 为 士 0. Imm。 此 外 ,由 于 机 器 人 运行 时 要 求 运动 平稳 ,不 受 外 
力 干 扰 , 故 系统 应 具有 较 好 的 刚性 。 

(4) 信息 运算 量 大 。 机 器 人 的 动作 住 往 可 以 通过 不 同 的 方式 和 路 径 来 完成 ,因此 存在 一 
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个 最 优 的 问题 , 较 高 级 的 机 器 人 可 以 采用 人 工 智 能 的 方法 ,用 计算 机 建立 起 庞大 的 信息 库 , 借 
助 信息 库 进 行 控制 ,决策 管理 和 操作 。 根 据 传感器 和 模式 识别 的 方法 获得 对 象 及 环境 的 工 况 ， 
按照 给 定 的 指标 要 求 , 自 动 选择 最 佳 的 控制 规律 。 

(5) 需 采用 加 ( 减 ) 速 控制 。 过 大 的 加 ( 减 ) 速 度 会 影响 机 器 人 运动 的 平稳 性 ,甚至 使 机 器 
人 发 生 拌 动 ,因此 应 在 机 器 人 起 动 或 停止 时 采取 加 ( 减 ) 速 控制 策略 。 通 常 采用 匀 加 ( 减 ) 速 运 
动 指令 来 实现 。 此 外 ,机 器 人 不 允许 有 位 置 超 调 ,否则 将 可 能 与 工件 发 生 碰撞 。 因 此 ,要 求 控 
制 系统 位 置 无 超 调 ,动态 响应 尽量 快 。 

(6) 工业 机 器 人 还 有 一 种 特有 的 控制 方式 一 一 示 教 再 现 控制 方式 。 当 要 工业 机 器 人 完成 
某 作 业 时 ,可 预先 移动 工业 机 器 人 的 手臂 来 示 教 该 作业 顺序 .位置 及 其 他 信息 ,在 此 过 程 中 把 
相关 的 作业 信息 存储 在 内 存 中 ,在 执行 任务 时 ,依靠 工业 机 器 人 的 动作 再 现 功能 ,可 重复 进行 
该 作业 。 此 外 ,从 操作 的 角度 来 看 ,要 求 控 制 系统 具有 良好 的 人 机 界面 ,尽量 降低 对 操作 者 的 
要 求 。 因 此 ,多 数 情况 要 求 控制 器 的 设计 人 员 不 仅 要 完成 底层 伺服 控制 器 的 设计 ,还 要 完成 规 
划算 法 的 编程 。 

综 上 所 述 ,工业 机 器 人 的 控制 系统 是 一 个 与 运动 学 和 动力 学 原理 密切 相关 的 有 耦合 的 、 
非 线性 的 多 变量 控制 系统 ,在 实际 工作 时 ,可 以 视 情 况 不 同 来 选择 不 同 的 控制 方式 。 


6.1.2 机 器 人 控制 系统 的 功能 

机 器 人 控制 系统 是 机 器 人 的 主要 组 成 部 分 ,用 于 控制 操作 机 来 完成 特定 的 工作 任务 ,其 基 
本 功能 有 示 教 -再 现 功能 .坐标 设置 功能 .与 外 围 设备 联系 的 功能 、 位 置 伺服 功能 。 

(1) 示 教 -再 现 功 能 。 机 器 人 控制 系统 可 实现 离线 编程 、 在 线 示 教 及 间接 示 教 等 功能 ,在 
线 示 教 又 包括 示 教 盒 示 教 和 导 引 示 教 两 种 情况 。 在 示 教 过 程 中 ,可 存储 作业 顺序 .运动 路 径 、 
运动 方式 .运动 速度 及 与 生产 工艺 有 关 的 信息 ,在 再 现 过 程 中 ,能 控制 机 器 人 按照 示 教 的 加 工 
信息 执行 特定 的 作业 。 

(2) 坐标 设置 功能 。 一 般 的 工业 机 器 人 控制 器 设置 有 关节 坐标 系 、 绝 对 坐标 系 、 工 具 坐 标 
系 及 用 户 坐 标 系 这 4 种 坐标 系 , 用 户 可 根据 作业 要 求 选用 不 同 的 坐标 系 并 进行 坐标 系 之 间 的 
转换 。 

(3) 与 外 围 设备 联系 的 功能 。 机 器 人 控制 器 设置 有 输入 /输出 接口 .通信 接口 .网 络 接口 
和 同步 接口 ,并 具有 示 教 盒 .操作 面板 及 显示 屏 等 人 机 接口 。 此 外 ,还 具有 多 种 传感器 接口 ,如 
视觉 .触觉 .接近 觉 .听觉 力 觉 (力矩 ) 传 感 器 等 多 种 传感器 接口 。 

(4) 位 置 伺服 功能 。 机 器 人 控制 系统 可 实现 多 轴 联 动 .运动 控制 .速度 和 加 速度 控制 力 


控制 及 动态 补偿 等 功能 。 在 运动 过 程 中 ,还 可 以 实现 状态 监测 ,故障 诊断 下 的 安全 保护 和 故障 . 


自 诊断 等 功能 。 


6.1.3 机 器 人 的 控制 方式 


根据 不 同 的 分 类 方法 ,机 器 人 控制 方式 可 以 有 不 同 的 分 类 。 总 体 上 ,机 器 人 的 控制 方式 可 
以 分 为 动作 控制 方式 和 示 教 控制 方式 。 按 运动 坐标 控制 可 以 分 为 关节 空间 运动 控制 和 直角 坐 
标 空间 的 运动 控制 。 按 照 运动 控制 方式 可 以 分 为 位 置 控制 * 速 度 控制 和 力 控制 (包括 位 置 与 力 
的 混合 控制 )。 这 里 根据 后 一 种 分 类 方法 ,对 工业 机 器 人 控制 方式 作 具 体 分 析 。 其 中 位 置 控制 
分 为 点 位 控制 和 连续 轨迹 控制 两 类 。 不 同 的 控制 方式 ,其 特点 和 应 用 也 不 同 。 

(1) 点 位 控制 方式 。 点 位 控制 方式 用 于 实现 点 的 位 置 控 制 , 其 运动 是 由 一 个 给 定点 到 另 
一 个 给 定点 ,而 点 与 点 之 间 的 轨迹 却 无 关 紧 要 。 因 此 ,这 种 控制 方式 的 特点 是 只 控制 工业 机 器 
人 末端 执行 器 在 作业 空间 中 某 些 规定 的 离散 点 上 的 位 置 和 姿态 。 控 制 时 只 要 求 工 业 机 器 人 快 


第 6 章 ”机 器 人 力 和 位 置 控制 | 加 61 


速 、 准 确 地 实现 相 邻 各 点 之 间 的 运动 ,而 对 达到 目标 点 的 运动 轨迹 则 不 做 任何 规定 ,如 自动 插 
件 机 ,在 贴 片 机 上 安插 元 件 , 点 焊 、 搬 运 .装配 等 作业 。 这 种 控制 方式 的 主要 技术 指标 是 定位 精 
度 和 完成 运动 所 需要 的 时 间 ,控制 方式 比较 简单 ,但 要 达到 较 高 的 定位 精度 则 较 难 。 

(2) 连续 轨迹 控制 方式 。 这 种 控制 方式 主要 用 于 指定 点 与 点 之 间 的 运动 轨迹 所 要 求 的 曲 
线 , 如 直线 或 圆 弧 。 这 种 控制 方式 的 特点 是 连续 地 控制 工业 机 器 人 末端 执行 器 在 作业 空间 中 
的 位 置 和 姿态 ,使 其 严格 按照 预先 设 定 的 轨迹 和 速度 在 一 定 的 精度 要 求 内 运动 ,一般 要 求 速度 
可 控 、. 运 动 轨迹 光滑 且 运 动 平 稳 , 以 完成 作业 任务 。 工 业 机 器 人 各 关节 连续 、 同 步 地 进行 相应 
的 运动 ,其 末端 执行 器 可 形成 连续 的 轨迹 。 这 种 控制 方式 的 主要 技术 指标 是 机 器 人 末端 执行 
器 的 轨迹 跟踪 精度 及 平稳 性 。 在 用 机 器 人 进行 弧 焊 喷漆 、 切 割 等 作业 时 ,应 选用 连续 轨迹 控 

制 方式 。 

(3) 速度 控制 方式 。 对 机 器 人 的 运动 控制 来 说 ,在 位 置 控制 的 同时 ,还 要 进行 速度 控制 ， 
即 对 于 机 器 人 的 行程 要 求 遵 循 一 定 的 速度 变化 曲线 。 例 如 ,在 连续 轨迹 控制 方式 下 ,机 器 人 按 
照 预 设 的 指令 ,控制 运动 部 件 的 速度 ,实现 加 、 减 速 , 以 满足 运动 平稳 .定位 精确 的 要 求 。 由 于 
工业 机 器 人 是 一 种 工作 情况 (行程 负载 ) 多 变 、 惯 性 负载 大 的 运动 机 械 , 控 制 过 程 中 必须 处 理 好 
快速 与 平稳 的 矛盾 ,必须 注意 起 动 后 的 加 速 和 停止 前 的 减速 这 两 个 过 渡 运 动 阶段 。 

(4) 力 (力矩 ) 控 制 方式 。 在 进行 抓 放 操 作 、 去 毛刺 .研磨 和 组 装 等 作业 时 ,除了 要 求 准确 
定位 之 外 ,还 要 求 使 用 特定 的 力 或 力矩 传感器 对 末端 执行 器 施加 在 对 象 上 的 力 进 行 控制 。 这 
种 控制 方式 的 原理 与 位 置 伺服 控制 原理 基本 相同 ,但 输入 量 和 输出 量 不 是 位 置信 号 ,而 是 力 
(力矩 ) 信 号 ,因此 系统 中 必须 有 力 ( 力 矩 ) 传 感 器 。 


6.2 机 器 人 的 位 置 控制 


工业 机 器 人 位 置 控制 的 目的 是 让 机 器 人 各 关节 实现 预期 规划 的 运动 ,最 终 保证 工业 机 器 
人 末端 执行 器 沿 预 定 的 轨迹 运行 。 对 于 机 器 人 的 位 置 控制 ,可 将 关节 位 置 给 定 值 与 当前 值 相 
比较 得 到 的 误差 作为 位 置 控制 器 的 输入 量 , 经 过 位 置 控 制 器 的 运算 后 ,将 输出 作为 关节 速度 控 
制 的 给 定 值 ,如 图 6.1 所 示 。 
关节 位 置 关节 


关节 位 置 0 


图 6.1 机 器 人 位 置 控制 示意 图 


工业 机 器 人 每 个 关节 的 控制 系统 都 是 闭环 控制 系统 。 对 于 工业 机 器 人 的 位 置 控 制 , 位 置 
检测 元 件 是 必 不 可 少 的 。 关 节 位 置 控制 器 常 采用 PID 算法 ,也 可 采用 模糊 控制 算法 等 智能 
六 法 

速度 控制 通常 用 于 对 目标 跟踪 的 任务 中 ,机 器 人 的 关节 速度 控制 示意 图 如 图 6. 2 所 示 。 
对 于 机 器 人 末端 笛 卡 儿 空 间 的 位 置 .速度 控制 ,其 基本 原理 与 关节 空间 的 位 置 和 速度 控制 
类 似 。 

虽然 工业 机 器 人 的 结构 多 为 串 接 连 杆 形式 , 且 其 动态 特性 一 般 具 有 高 度 的 非 线性 ,但 是 在 
设计 其 控制 系统 时 ,通常 把 机 器 人 的 每 个 关节 当 作 一 个 独立 的 伺服 机 构 来 考虑 。 这 主要 是 因 
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关节 位 置 关节 


给 定 值 速度 放大 2 
关节 速度 6 


图 6.2 机 器 人 关节 速度 控制 示意 图 


为 工业 机 器 人 运动 速度 偏 低 (一 般 小 于 1. 5m/s) ,因此 可 以 忽略 由 速度 变化 引起 的 非 线性 影 
响 。 另 外 ,由 于 交流 伺服 电机 都 安装 有 减速 器 ,通常 其 减速 比 接近 100, 当 负载 变化 时 ,折算 到 
电机 轴 上 的 负载 变化 值 则 很 小 ,所 以 负载 变化 的 影响 可 以 忽略 。 而 且 由 于 减速 器 的 存在 , 极 大 
地 削弱 了 各 关节 之 间 的 看 合作 用 ,因此 可 以 将 工业 机 器 人 系统 看 成 一 个 由 多 关节 组 成 的 .相互 
独立 的 线性 系统 。 

下 面 分 析 以 伺服 电动 机 为 驱动 器 的 独立 关节 的 控制 问题 。 


6.2.1 单 关 节 位 置 控制 

1. 基于 直流 伺服 电动 机 的 单 关 节 位 置 控 制 

单 关 节 控 制 器 是 指 不 考虑 关节 之 间 的 相互 影响 ,只 根据 一 个 关节 独立 设置 的 控制 器 。 在 
单 关节 控制 器 中 ,机 器 人 的 机 械 惯性 影响 常常 作为 扰动 项 来 考虑 ,把 机 器 人 看 作 刚 体 结构 。 直 
流 伺服 电动 机 的 位 置 控制 有 两 种 方式 ,一 种 是 位 置 加 电流 反馈 的 双环 结构 , 另 一 种 是 位 置 、 速 
度 加 电流 反馈 的 三 环 结构 。 无 论 采用 哪 种 结构 形式 ,都 需要 从 直流 伺服 电动 机 数学 模型 人 手 ， 
对 系统 进行 分 析 。 由 电机 、 齿 轮 和 负载 组 成 的 单 关节 电动 机 负载 模型 如 图 6. 3 所 示 。 


Co 


图 6.3 单 关 节 直 流 伺 服 电动 机 负载 模型 


对 图 中 参数 的 说 明 如 下 : 

0. 一 一 负载 轴 的 角 位 移 ,单位 为 rad; 

0 一 一 驱动 轴 角 位 移 ,rad; 

7 一 0 /0. 一 一 齿轮 减速 比 ; 

有 负载 侧 的 总 转 和 矩 ,单位 为 N。my 

T, 一 一 直流 伺服 电动 机 输出 转 矩 ,N。m 

有 负载 轴 的 总 转动 惯量 , 单位 为 kg。m” ; 

J], 一 一 关节 部 分 在 齿轮 箱 驱 动 侧 的 转动 惯量 , kg *m ; 

J] ,一 一 电动 机 转子 转动 惯量 , kg m2 ; 

也 .一 一 负载 轴 的 阻尼 系数 ; 

也 ,一 一 驱动 侧 的 阻尼 系数 。 

以 上 参数 用 来 研究 负载 转角 0, 与 电动 机 的 电 枢 电压 U 之 间 的 传递 函数 ,下 面 这 些 参数 
在 研究 0, 与 U 之 间 的 传递 函数 时 也 将 会 用 到 ,在 此 一 起 说 明 。 
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1 一 一 电 枢 绕组 电流 ,单位 为 A; 

LL 一 一 电 枢 电感 ,单位 为 Hi 

R 一 一 电 枢 电阻 ,单位 为 Q; 

Kc 一 一 电动 机 的 转 矩 常数 ,单位 为 N.，m/A; 

K ,一 一 电动 机 反 电 动 势 常数 ,单位 为 V，s/rad; 

J uf 二 J 4 十] 十 J 一 一 电动 机 轴 上 的 等 效 转动 惯量 ; 
Bu 二 B, 十 7*B, 一 一 电动 机 轴 上 的 等 效 阻尼 系数 ; 

Ky 一 一 转换 常数 ,单位 为 V/rad。 


已 知 电动 机 输出 转 矩 为 
， 工 。 王 开 cTCON m) (6.1) 
电 枢 绕组 电压 平衡 方程 为 
Kd Ldi 
以 三 地 qd el (6.2) 


对 式 (6.1) 和 式 (6. 2) 两 边 同 时 作 拉 普 拉 斯 变换 ,整理 可 得 
一 0 


Tt =K¢c WO CB. 
驱动 轴 ( 即 电动 机 输出 轴 ) 的 转 抢 平衡 方程 为 
CJ 十 J。)d20，。 d0， 
T, 一 -一 一 一 一 十 B。 一 一 十 7T。 (6.4) 
dt dt 
负载 轴 的 转 矩 平衡 方程 为 
T. = 十 B 0 (6.5) 
ss 2 : 
对 式 (6.4) 和 式 (6. 5) 分 别 作 拉 普 拉 斯 变换 可 得 
T,(s) =(], 十 J )s20 Cs) + B,s0, Cs) + rT. Cs) (6. 6) 
rR 学 和 从 (6.7) 


对 7 一 0, /0. 进行 拉 普 拉 斯 变换 可 得 0,,(s) 二 9.(s)/w, 将 其 代入 式 (6.6) 并 联合 式 (6. 3) 和 
式 (6.7) 整 理 可 得 
人 
式 (6. 8) 描 述 了 输入 控制 电压 与 驱动 轴 角 位 移 9,, 的 关系 。 该 式 右边 分 母 上 括号 外 的 * 表 
示 当 施加 电压 U 后 ,0,, 也 是 对 时 间 t 的 积分 ; 方 括号 内 的 表达 式 表示 该 系统 是 一 个 二 阶 速 度 
控制 系统 。 由 于 ww 一 d0, /dt ,进行 拉 普 拉 斯 变换 可 得 w,, (s) 二 50,,(s) ,将 式 (6. 8) 中 左边 分 
母 上 的 ; 移 项 可 得 
sOm (5) wns) _ Ke i 
U(s) U(s) JuLs:+(JuR+i+BuaL)s+BuaR+Kek, , 
为 了 构成 对 负载 轴 的 角 位 移 控 制 器 ,必须 进行 负载 轴 的 角 位 移 反 馈 , 即 用 某 一 时 刻 t 所 需 
要 的 角 位 移 9, 与 实际 角 位 移 0. 之 差 所 产生 的 电压 来 控制 该 系统 。 用 光学 编码 器 作为 实际 位 
置 传感器 ,可 以 求 取 位 置 误差 ,误差 电压 为 
U(1) =K,(0,—0.) (6. 10) 
邻 巨 (1) 二 04 (1) 一 9,(1),0.(t) 二 0,,(t) 对 这 3 个 表达 式 分 别 进行 拉 普 拉 斯 变换 可 得 


0 Cs Ke 


(6. 8) 
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U(s) =Ko,L0s(s) — 0,.(s)] (6. 11) 
E(s)=07(s)—0.Cs) (6. 12) 
0.(s) =70,(s) 、 Cb: 13) 


从 理论 上 讲 , 式 (6.9) 表 示 的 二 阶 系 统 是 稳定 的 。 要 提高 响应 速度 ,可 以 调 高 系统 的 增益 
(如 增 大 K,) 及 电动 机 传动 轴 速 度 负 反馈 ,把 某 些 阻 尼 引 入 到 系统 中 ,以 加 强 反 电动 势 的 作用 
效果 。 要 做 到 这 一 点 ,可 以 采用 测速 发 电机 ,或 计算 一 定时 间 间 隔 内 传动 轴 角 位 移 的 差 值 。 单 
关节 位 置 控制 器 如 图 6.4(a) 所 示 。 如 图 6. 4(b) 所 示 为 具有 速度 反馈 功能 的 位 置 控制 器 ,其 
中 ,K, 为 测速 发 电机 的 传递 系数 (单位 为 V，s/rad),K 为 速度 反馈 信号 放大 器 的 增益 。 由 
于 电动 机 电 枢 回路 的 反馈 电压 已 经 由 KK,0,, (1) 增 加 为 K,0, (1) 十 KiK,0,(t)= 二 (K++ 
KiK,)0,,(t), 所 以 其 对 应 的 开 环 传递 函数 为 
Gs) nKoKre 
E(s) ssSLLJus + (RJ LBu)s + RB KeK,] 
机 器 人 驱动 电动 机 的 电感 L (一 般 为 10mH) 远 小 于 电阻 R( 约 19) ,因此 可 以 略 去 式 (6. 14) 
中 的 工 , 式 (6. 14) 变 为 


(6. 14) 


OS 7nKoKe 
E(s) s[LRJus+RBut+KeK.,] 


(b) 具有 速度 反馈 功能 的 位 置 控制 器 
Fm(s) Ti(s) TsGs) 离心 力作 用 


(0) 考虑 摩擦 力矩 、 外 负载 力矩 、 重 力矩 及 向 心力 作用 的 位 置 控 制 器 
图 6.4 单 关节 机 械 辟 位 置 控 制 器 结构 
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图 6.4(a) 的 单位 反馈 位 置 控制 系统 的 闭环 传递 函数 是 
ke nKoKe 
04(s) 1+0./E RJss + (RB KeK,)s+rKoKe 
如 图 6.4(c) 所 示 考 虑 了 摩擦 力矩 、 外 负载 力矩 、 重 力矩 及 向 心力 作用 的 位 置 控 制 器 。 以 
任 一 扰动 作为 干扰 输入 ,可 写 出 干扰 的 输出 函数 与 传递 函数 。 利 用 拉 普 拉 斯 变换 中 的 终 值 定 
理 , 即 可 求 出 因 干 扰 引 起 的 静态 误差 。 
2. 带 力矩 的 单 关节 位 置 控制 
带 有 力矩 闭环 的 单 关 节 位 置 控制 系统 是 一 个 三 闭环 控制 系统 ,由 位 置 环 .力矩 环 和 速度 环 
构成 ,如 图 6.5 所 示 。 


(6.16) 


关节 位 置 
图 6.5 带 有 力矩 闭环 的 单 关节 位 置 控制 系统 


速度 环 为 控制 系统 的 内 环 , 其 作用 是 通过 对 控制 电动 机 的 电压 使 电动 机 表现 出 期 望 的 速 
度 特性 ,速度 环 的 给 定 值 是 力矩 环 偏差 经 过 放大 后 的 输出 (电动 机 角速度 为 2. ) ,速度 环 的 反 
馈 值 是 关节 角速度 2 ,将 Q, 与 92， 的 偏差 作为 电动 机 电压 驱动 器 的 输入 ,经 过 放大 后 成 为 
电压 U ,其 中 天 ,表示 转换 常数 ( 即 比例 系数 )。 电 动机 在 电压 U 的 作用 下 ,以 角速度 2， 旋 
转 。1/(L, 十 R) 为 电动 机 的 电磁 惯性 环节 ,其 中 ,L 为 电 枢 电感 ,R 为 电 枢 电阻 ,T 为 电 枢 电 
流 。 考 虑 到 一 般 情况 下 ,L 三 R, 故 一 般 可 以 忽略 电感 工 的 影响 ,环节 1/(L, 十 R) 守 1/R。. 
1/(J urs 十 B) 是 电动 机 的 机 电 惯 性 环节 ,Kc 为 电流 力矩 常数 , 即 电动 机 力矩 工 ,, 与 电 枢 电流 了 
之 间 的 系数 。 

位 置 环 为 控制 系统 的 外 环 , 用 于 控制 关节 以 达到 期 望 的 位 置 。 位 置 环 的 给 定 值 是 期 望 的 
关节 位 置 9, ,反馈 为 关节 位 置 0, ,将 09, 与 6， 的 偏差 作为 位 置 调节 器 的 输入 ,经 过 位 置 调节 
器 运算 后 形成 的 输出 作为 力矩 环 给 定 值 的 一 部 分 ,位 置 调 节 器 常 采 用 PID 或 PI 控制 器 ,构成 
的 位 置 闭环 系统 为 无 静 差 系统 。 


6.2.2 机 器 人 的 多 关节 控制 
1. 机 器 人 系统 的 伺服 控制 律 
因为 机 器 人 的 机 械 臂 具有 多 个 关节 ,分 别 需 要 相应 的 驱动 电机 提供 驱动 力矩 ,并 输出 多 个 
关节 的 位 置 .速度 和 加 速度 ,所 以 对 机 械 臂 进行 控制 是 一 个 多 输入 多 输出 的 问题 。 
将 控制 律 分 解 为 基于 模型 的 控制 部 分 和 伺服 控制 部 分 ,那么 它 可 以 表示 为 : 
下 一 aoF 十 8 (6 全 
其 中 ,下 FE 8 为 2X1 的 矢量 ,a 为 n Xn 的 矩阵 。B 为 基于 模型 的 控制 部 分 、 而 为 伺服 控制 
部 分 ,可 以 表示 为 : 
F’=X%i 1K,E+K,E (6. 18) 
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其 中 ,K, 、K, 为 n Xn 的 矩阵 ,E 为 nX1 的 位 置 误差 矢量 ,为 nX1 的 速度 误差 矢量 。 
2. 基于 模型 机 械 禹 控制 
考虑 摩擦 等 非 刚 体 效应 影响 的 机 器 人 动力 学 模型 为 
r=M(g)y+C(g0y+G(g) + Fd) (6. 19) 
其 中 ,M(Co)ER" ~"” 为 机 器 人 的 惯性 矩阵 ; C(q ,0yER"” 为 科 里 奥 利 和 矩阵 ; G(g)ER” 表 示 重 
力矩 阵 ; F(9 ) 为 摩擦 力矩 。 
令 
rz 一 ar 十 有 (6. 20) 
其 中 
a=M(g) 
es 《65 21) 
rz 一 和 十 下 ,六 十 下 ,五 
式 中 ,z“ 为 关节 力矩 ;误差 为 下 一 qu 一 9, 忆 一 gu 一 4。 
控制 系统 的 结构 框图 如 图 6. 6 所 示 。 


图 6.6 “基于 动力 学 模型 的 控制 系统 框图 


由 式 (6. 19) 一 式 (6.21) 可 以 得 出 表示 闭环 系统 的 误差 方程 


E+KjE+K,E=0 (6. 22) 
由 于 增益 矩阵 天。 和 天 ,是 对 角形 ,因而 上 式 是 解 耦 的 ,并 可 写成 ”个 单 关节 的 形式 
着 ; +K,E+K,E=0 (i=1,2,.,n) (6. 23) 


实际 上 ,由 于 系统 动态 模型 不 准确 等 原因 , 式 (6. 22) 所 表示 的 是 理想 情况 。 
6.3 机 器 人 的 力 控制 


机 器 人 的 力 控制 着 重 研究 如 何 控制 机 器 人 的 各 个 关节 ,使 其 末端 表现 出 一 定 的 力 和 力矩 
特性 ,是 利用 机 器 人 进行 自动 加 工 ( 如 装配 等 ) 的 基础 。 

当 机 器 人 在 空间 跟踪 轨迹 运动 时 ,可 采用 位 置 控制 ,但 当 机 器 人 在 完成 一 些 与 环境 存在 力 
作用 的 任务 时 ,例如 打磨 、 装 配 ,单纯 的 位 置 控制 会 由 于 位 置 误差 而 引起 过 大 的 作用 力 , 从 而 伤 
害 零件 或 机 器 人 。 机 器 人 在 这 类 运动 受 限 环境 中 运动 时 ,往往 需要 配合 力 控制 来 使 用 。 位 置 
控制 下 ,机 器 人 会 严格 按照 预先 设 定 的 位 置 轨迹 进行 运动 。 若 机 器 人 运动 过 程 中 遭遇 到 障碍 
物 的 阻拦 ,从 而 导致 机 器 人 的 位 置 追 踪 误 差 变 大 ,此 时 机 器 人 会 努力 地 “出 力 ” 去 追踪 预 设 轨 
迹 , 最 终 导致 机 器 人 与 障碍 物 之 间 巨 大 的 内 力 。 而 在 力 控制 下 ,以 控制 机 器 人 与 障碍 物 间 的 作 
用 力 为 目标 。 当 机 器 人 遭遇 障碍 物 时 ,会 智能 地 调整 预 设 位 置 轨迹 ,从 而 消除 内 力 。 
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当 工 业 机 器 人 末端 执行 器 与 环境 相 接 触 时 ,会 产生 相互 作用 的 力 , 如 图 6.7 所 示 。 一 般 情 
况 下 ,存在 接触 力 时 ,必须 建立 某 种 环境 作用 模型 。 为 使 概念 明确 ,用 类 似 于 位 置 控制 的 简化 
方法 ,假设 系统 是 刚性 的 ,质量 为 m ,而 环境 刚度 为 &。, 采 用 简单 的 质量 -弹簧 模型 来 表示 受 控 
物体 与 环境 之 间 的 接触 作用 ,如 图 6. 8 所 示 。 


图 6.7 机 器 人 与 环境 的 相互 作用 图 6.8 质量 -弹簧 系统 


下 面 重点 讨论 质量 -弹簧 系统 的 力 控 制 问题 。 

对 图 6. 8 中 主要 参数 的 规定 如 下 : 

太一 一 表示 未 知 的 干扰 力 ,通常 为 模型 未 知 的 摩擦 力 或 者 机 械 传 动 的 阻力 

太一 一 表示 作用 于 环境 的 力 , 也 是 施加 在 弹簧 上 的 力 , 它 与 环境 刚度 之 间 的 关系 如 下 


六 (6. 24) 
描述 该 物理 系统 的 方程 为 
f=mzi+k.zrzt fa (6. 25) 
如 果 用 作用 在 环境 上 的 控制 变量 f。 表示 ,形式 如 下 
f=mki!f, Ff, 二 fy (6. 26) 
采用 控制 律 分 解 的 方法 , 令 
a =mk,! 
(6 27) 
B=f., he 


从 而 得 到 控制 律 , 即 


f=mk, (fn tke i Ryser) + Ft fa (6. 28) 
式 中 ,ej 二 了 ,一 了。 为 期 望 力 f, 与 用 力 传感器 检测 到 的 环境 作用 力 f。 之 间 的 误差 。&。p 和 
pf 则 为 力 控制 系统 的 增益 系数 。 
如 果 式 (6. 28) 中 干扰 fj 是 已 知 的 , 则 联 立 式 (6. 24) ,可 得 闭环 系统 的 误差 方程 为 
és 十 Eurer 十 Rsrer 一 0 (6. 29) 
一 般 情况 下 ,在 控制 律 中 干扰 fj 是 未 知 的 ,因此 式 (6. 28) 不 可 解 。 但 是 ,可 以 在 指定 伺 
服 规则 时 ,去 掉 干扰 f 4 , 即 令 式 (6. 24) 和 式 (6. 28) 的 右边 相等 ,并 且 在 稳 态 分 析 中 令 对 时 间 的 
各 阶 导 数 都 为 零 ,可 以 得 到 如 下 关系 


er -全 (6. 30) 
式 中 ,a 二 mk。 or 为 有 效力 反馈 增益 。 如 果 用 f 取代 式 (6. 28) 中 的 fi 十 fa , 则 可 得 
ep -i G631» 


一 般 情 况 下 ,环境 刚度 有 。 比较 大 ,也 就 意味 着 a 的 值 可 能 会 比较 小 ,因此 优选 式 (6. 31) 计 
算 稳 态 误差 。 此 时 ,控制 律 如 下 


f =mk (fy, hes kyrer) tt fa (6 :132) 


如 图 6. 9 所 示 为 采用 控制 律 即 式 (6. 32) 的 闭环 力 控制 系统 原理 图 。 该 图 描述 的 力 伺服 控 
制 是 理想 情况 ,实际 的 力 伺服 控制 通常 有 些 不 同 。 实 际 情况 中 , 力 轨迹 一 般 为 常数 ,这 是 由 于 
通常 希望 接触 力 为 菜 一 常数 值 ,很 少将 它 设置 为 时 间 的 函数 。 因 此 ,图 6.9 中 系统 的 输入 ff， 
和 f， 通常 设置 恒 为 零 。 另 外 ,实际 情况 中 检测 到 的 力 “ 噪 声 ” 很 大 ,通过 数值 微分 的 方法 计算 
广 是 行 不 通 的 。 因 为 f, 二 kx ,因此 可 以 通过 求解 作用 于 环境 上 的 力 的 微分 广 . =&. 之 来 求 
广  。 这 是 由 于 大 多 数 机 械 臂 都 可 以 测量 速度 ,而且 技术 成 熟 。 综 上 ,可 以 将 控制 律 写 为 

R=m yk, er — bud) Fa (6. 33) 
与 式 (6. 33) 对 应 的 原理 图 为 6. 10。 


图 6.10 实际 的 质量 -弹簧 系统 的 力 控制 系统 原理 图 


6.4 机 器 人 的 力 / 位 混合 控制 
本 节 将 介绍 力 /位 混合 控制 器 的 控制 系统 结构 。 


6.4.1 力 /位 混合 控制 问题 的 提出 

机 器 力 控制 的 最 佳 方案 是 以 独立 的 形式 同时 控制 力 和 位置 ,理论 上 力 自 由 空间 和 位 置 自 
由 空间 是 2 个 互补 的 正 交 子 空间 ,在 力 自由 空间 进行 力 控 制 , 而 在 剩余 的 正 交 方向 上 进行 位 置 
控制 。 此 时 的 约束 环境 被 当 作 不 变形 的 几何 问题 考虑 ,因此 也 可 狭义 地 称 为 约束 运动 控制 。 

机 器 人 末端 执行 器 与 外 界 环境 接触 时 有 两 种 极端 情况 。 一 种 是 机 器 人 末端 执行 器 在 空间 
可 以 自由 移动 ,如 图 6. 11(a) 所 示 。 在 这 种 情况 下 ,自然 约束 都 是 关于 接触 力 的 约束 , 且 所 有 
约束 力 都 为 零 。 换 言 之 ,不 能 在 末端 执行 器 的 任何 方向 上 施加 力 , 但 是 可 以 在 位 置 的 6 个 
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自由 度 上 运动 。 另 一 种 情况 是 ,末端 执行 器 固定 不 ne 
动 , 即 其 不 能 自由 地 改变 位 置 ,为 操作 臂 末 端 执 行 器 
紧 贴 墙 面 运动 的 极端 情况 ,如 图 6. 11(b) 所 示 。 在 这 中 
种 情况 下 ,对 机 械 臂 未 端 执行 器 的 自然 约束 是 6 个 自 N 
然 位 置 约束 ,但 可 以 在 这 6 个 自由 度 上 对 其 施加 力 和 
力矩 。 

上 述 两 种 极端 情况 中 ,第 一 种 情况 为 位 置 控制 问 
题 , 第 二 种 情况 在 实际 中 并 不 经 常 出 现 ,通常 多 数 情况 四 中 
是 对 系统 的 某 些 自由 度 需要 进行 位 置 控制 ,而 对 另外 ”图 6.11 末端 执行 器 与 外 界 环境 
一些 自由 度 则 需要 进行 力 控制 。 这 时 ,就 需要 采用 力 接触 的 两 种 极端 情况 
和 位 置 混合 控制 的 方式 。 

机 器 人 的 力 /位 置 混合 控制 器 必须 解决 以 下 3 个 问题 。 

(1) 对 有 力 自然 约束 的 方向 需要 施加 位 置 控制 。 

(2) 对 存在 位 置 自然 约束 的 方向 应 施加 力 控制 。 

(3) 在 任意 约束 坐标 系 {C} 的 正 交 自 由 度 上 应 进行 力 和 位 置 的 混合 控制 。 

6.4.2 以 坐标 系 {C) 为 基准 的 直角 坐标 机 械 锅 的 力 /位 混合 控制 

系统 

考虑 一 台 简 单 的 三 自由 度 的 直角 坐标 型 的 机 械 臂 ,如 图 6. 12 所 示 。 该 机 械 臂 的 3 个 关节 
均 为 移动 关节 , 每 个 连 杆 的 质量 均 为 m ,滑动 摩擦 力 为 零 。 假 设 关 节 轴 线 义 .Y 和 2 的 方向 与 
约束 坐标 系 {C} 的 轴线 方向 完全 一 致 。 末 端 执行 器 与 刚度 为 k, 的 表面 接触 ,作用 在 CY 方向 
上 。 因 此 ,CY 方向 需要 进行 力 控制 ,而 在 CX 和 CZ 方向 则 需要 设 定位 置 控制 。 

如 果 希 望 将 约束 表面 的 法 线 方向 转变 为 沿 X 方向 或 者 Z 方向 , 则 需要 对 直角 坐标 型 机 械 
臂 控制 系统 稍 加 扩展 ,具体 方法 为 : 构建 一 个 控制 器 ,使 它 可 以 确定 3 个 自由 度 的 全 部 位 置 轨 
迹 ,同时 也 能 确定 3 个 自由 度 的 力 轨迹 。 当 然 , 不 能 同时 满足 这 6 个 约束 的 控制 。 因 此 ,需要 
设 定 一 些 工作 模式 来 指明 在 任 一 给 定时 刻 应 控制 哪 条 轨迹 的 哪个 分 量 。 

在 如 图 6. 12 所 示 的 控制 器 中 ,用 一 个 位 置 控 制 器 和 一 个 力 控制 器 对 上 述 简单 直角 坐标 型 
机 械 臂 的 3 个 关节 进行 控制 。 在 此 ,通过 引入 矩阵 $S 和 Ss’ 的 方式 确定 应 采用 的 控制 模式 一 一 
位 置 或 力 , 进 而 控制 直角 坐标 型 机 械 臂 的 每 一 个 关节 。S$ 矩阵 为 对 角 阵 ,对 角 线 上 的 元 素 为 1 
和 0。 对 于 位 置 控制 ,S 中 元 素 为 1 的 位 置 在 S$’ 中 对 应 的 元 素 为 0; 对 于 力 控制 ,S 中 元 素 为 0 
的 位 置 在 S' 中 对 应 的 元 素 为 1。 因 此 ,可 以 把 矩阵 S$ 和 Ss 看 成 一 个 互 锁 开 关 , 用 来 设 定 坐 标 系 
{C} 中 每 一 个 自由 度 的 控制 模式 。 按 照 S 的 规定 ,系统 中 有 3 个 轨迹 分 量 受到 控制 ,而 位 置 控 
制 和 力 控制 之 间 的 组 合 是 任意 的 。 另 外 3 个 期 望 轨迹 分 量 和 相应 的 伺服 误差 应 被 忽略 , 即 当 
一 个 给 定 的 自由 度 受 到 力 控 制 时 , 则 该 自由 度 上 的 位 置 误差 应 该 被 忽略 。 


6.4.3 ”应 用 于 一 般 机 械 苞 的 力 / 位 混合 控制 系统 


如 图 6. 12 所 示 的 混合 控制 器 是 关节 轴线 与 约束 坐标 系 {C } 完 全 一 致 的 特殊 情况 。 将 这 
种 研究 方法 推广 到 一 般 机 械 臂 ,以 便 直接 应 用 基于 直角 坐标 系 的 控制 方法 。 其 基本 思路 是 使 
用 直角 坐标 空间 的 动力 学 模型 ,把 实际 机 械 臂 的 组 合 系统 和 计算 模型 变换 为 一 系列 独立 的 、 解 
耦 的 单位 质量 系统 。 一 旦 完成 解 耦 和 线性 化 ,就 可 以 应 用 前 面 所 介绍 的 简单 伺服 方法 来 进行 
综合 分 析 。 
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图 6.12 三 自由 度 直角 坐标 型 机 械 臂 的 混合 控制 器 


在 直角 坐标 空间 中 机 械 臂 动力 学 公式 的 解 耦 形式 如 图 6. 13 所 示 , 它 使 机 械 辟 呈现 为 一 系 
列 解 耦 的 单位 质量 系统 。 为 了 用 于 混合 控制 策略 ,应 在 约束 坐标 系 {C} 中 描述 直角 坐标 空间 
动力 学 方程 和 雅 可 比 矩 阵 。 


V(0, 0)+GO) 


图 6.13 直角 坐标 解 耦 方法 


由 于 已 经 设计 了 一 个 与 约束 坐标 系 一 致 的 直角 坐标 型 机 械 臂 的 混合 控制 器 ,并 且 因为 用 
直角 坐标 解 耦 方法 建立 的 系统 具有 相同 的 输入 -输出 特性 ,因此 只 需要 将 这 两 个 条 件 结合 ,就 
可 以 生成 一 般 的 力 /位 混合 控制 器 。 

一 般 机 械 臂 的 混合 控制 器 框图 如 图 6. 14 所 示 。 需 要 注意 的 是 ,动力 学 方程 以 及 雅 可 比 拢 
阵 都 在 约束 坐标 系 {C } 中 描述 。 描 述 运动 学 方程 时 ,也 需要 将 坐标 变换 到 约束 坐标 系 。 同 样 
地 ,检测 的 力也 应 变换 到 约束 坐标 系 中 。 另 外 ,伺服 误差 也 应 在 {C}) 中 计算 ,当然 还 要 适当 选 
择 S 和 S 的 值 ,以 确定 控制 模式 。 


图 6.14 一 般 机械 臂 的 力 /位 混合 控制 器 
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6.5 本 章 小 结 


本 章 首 先 对 机 器 人 控制 系统 的 一 些 基 本 概念 进行 了 阐述 ,然后 分 别 重点 介绍 了 机 器 人 的 
位 置 . 力 以 及 力 / 位 混合 这 三 种 不 同 的 控制 器 。 对 于 机 器 人 的 位 置 控制 ,以 直流 伺服 电动 机 为 
例 , 分 别针 对 单 关节 和 多 关节 的 情况 进行 讨论 ; 关于 机 器 人 的 力 控 制 , 本 章 以 典型 的 质量 - 弹 
簧 系统 为 例 ,讨论 其 控制 器 的 设计 方法 ; 最 后 ,综合 以 上 两 种 控制 器 的 使 用 场景 ,讨论 力 /位 混 
合 控制 器 的 问题 ,包括 以 {C } 为 基准 的 直角 坐标 机 械 臂 的 力 /位 混合 控制 系统 和 应 用 于 一 般 机 
械 臂 的 以 {C } 为 基准 的 直角 坐标 机 械 臂 的 力 /位 混合 控制 。 本 章 介绍 的 关于 机 器 人 控制 的 方 
法 比较 传统 ,本 书 第 二 篇 将 对 一 些 现代 的 .新颖 的 .智能 的 机 器 人 控制 方法 进行 介绍 ,并 针对 每 
- 种 控制 方法 给 出 相应 的 应 用 研究 实例 。 

本 篇 主要 为 机 器 人 相关 的 理论 基础 知识 ,是 作者 在 现 有 知识 积累 的 基础 上 参考 大 量 专 著 
编写 而 成 。 由 于 不 同 章节 之 间 的 参考 文献 有 交叉 ,因此 统一 列 出 部 分 主要 参考 文献 ,每 章 结束 
时 不 再 单独 附 上 相应 参考 文献 。 


有 


2 万 污 篇 : 
树 刺 圳 忆 计 与 仿真 


第 一 篇 主要 介绍 了 机 器 人 学 的 基础 理论 知识 ,其 中 也 涉及 一 些 传 统 的 机 器 人 控制 器 设计 
方法 ,如 机 器 人 的 力 控制 ,位 置 控制 以 及 力 / 位 混合 控制 等 。 本 篇 将 研究 机 器 人 领域 比较 先进 
的 、 常 用 的 控制 方法 ,如 自 适应 控制 、 变 结构 控制 以 及 智能 控制 等 。 

按照 控制 算法 的 不 同 , 机 器 人 的 控制 方法 可 以 分 为 PID 控制 \ 变 结构 控制 、 自 适应 控制 、 
模糊 控制 .神经 元 网 络 控 制 等 方法 。 也 有 学 者 将 现 有 的 控制 算法 分 为 逻辑 门限 控制 .PID 控 
制 、 滑 模 变 结 构 控 制 、 神 经 网 络 控制 和 模糊 控制 等 。 这 些 控制 方法 并 非 孤 立 的 ,在 一 个 控制 系 
统 之 中 常常 是 结合 在 一 起 使 用 的 。 下 面 介 绍 几 种 常用 的 机 器 人 控制 方法 。 

1. PID 控制 

在 实际 工程 中 ,应 用 最 为 广泛 的 调节 器 控制 规律 为 比例 、 积 分 、 微 分 (Proportion-Integral 
Derivative, PID) 控 制 ,又 称 PID 调节 。PID 控制 器 自问 世 至 今 已 有 近 70 年 历史 , 它 因 结构 简 
单 .稳定 性 好 、 工 作 可 靠 、 调 整 方便 而 成 为 工业 控制 的 主要 技术 之 一 。 当 被 控 对 象 的 结构 和 参 
数 不 能 完全 掌握 ,或 得 不 到 精确 的 数学 模型 ,控制 理论 的 其 他 技术 难以 采用 时 ,系统 控制 器 的 
结构 和 参数 必须 依靠 经 验 和 现场 调试 来 确定 ,这 时 应 用 PID 控制 技术 最 为 方便 。 即 当 我 们 不 
完全 了 解 一 个 系统 和 被 控 对 象 , 或 不 能 通过 有 效 的 测量 手段 来 获得 系统 参数 时 ,最 适合 采用 
PID 控制 技术 。PID 控制 在 实际 中 也 有 PI 和 PD 控制 。PID 控制 器 是 根据 系统 的 误差 ,利用 
比例 、 积 分 、 微 分 计算 出 控制 量 进行 控制 的 。 这 种 控制 方法 的 控制 律 简单 ,易于 实现 ,不 用 建 
模 ,但 是 难以 保证 机 器 人 具有 良好 的 动态 和 静态 品质 , 且 需 要 较 大 的 控制 能 量 。 

2. 变 结构 控制 

变 结 构 控 制 是 20 世纪 50 年 代 从 苏联 发 展 起 来 的 一 种 控制 方法 。 所 谓 变 结构 控制 ,是 指 
控制 系统 中 具有 多 个 控制 器 ,根据 一 定 的 规则 在 不 同情 况 下 采用 不 同 的 控制 器 。 采 用 变 结构 
控制 具有 许多 其 他 控制 所 没有 的 优点 ,可 以 实现 对 一 类 具有 不 确定 参数 的 非 线 性 系统 的 控制 。 


3. 自 适 应 控制 

自 适应 控制 ,是 指 系统 的 输入 或 干扰 发 生 较 大 范围 的 变化 时 ,所 设计 的 系统 能 够 自 适应 调 
节 系 统 参数 或 控制 策略 ,使 输出 仍 能 达到 设计 要 求 。 自 适应 控制 所 处 理 的 是 具有 “不 确定 性 ” 
的 系统 ,通过 对 随机 变量 状态 的 观测 和 系统 模型 的 辨识 ,设法 降低 这 种 不 确定 性 。 当 机 器 人 动 
力学 模型 存在 非 线性 和 不 确定 性 因素 时 ,如 未 知 的 系统 参数 、 非 线性 动态 特性 以 及 环境 因素 
等 ,采用 自 适应 控制 来 补偿 上 述 因素 ,能 够 显著 改善 机 器 人 的 性 能 。 自 适应 控制 器 是 机 器 人 控 
制 器 设计 的 一 种 可 行 且 有 效 的 方法 。 

自 适 应 控制 系统 按 其 原理 的 不 同 , 可 分 为 模型 参考 自 适应 控制 系统 、 自 校正 控制 系统 、 自 
寻 优 控制 系统 、 变 结构 控制 系统 和 智能 自 适应 控制 系统 等 。 在 这 些 自 适应 控制 系统 中 ,模型 参 
考 自 适 应 控制 系统 和 自 校 正 控制 系统 较 成 熟 , 也 较 常 用 。 

4. 模糊 控制 

模糊 控制 是 模糊 逻辑 控制 的 简称 ,是 以 模糊 集合 论 、 模 糊 语言 变量 和 模糊 逻辑 推理 为 基础 
的 一 种 计算 机 数字 控制 技术 。 模 糊 控 制 可 以 代替 经 典 控制 系统 或 与 经 典 控制 系统 相 结 合 对 机 
器 人 进行 控制 。 模 糊 控制 的 基本 思想 是 将 人 类 专家 对 特定 对 象 的 控制 经 验 , 运 用 模糊 集 理 论 
进行 量化 ,转化 为 可 用 数学 实现 的 控制 器 ,从 而 实现 对 被 控 对 象 的 控制 。 在 模糊 控制 中 ,输入 
量 经 过 模糊 量化 成 为 模糊 变量 ,由 模糊 变量 经 过 模糊 规则 的 推理 获得 模糊 输出 ,经 过 解 模糊 得 
到 清晰 的 输出 量 用 于 控制 。 模 糊 控制 最 早 在 1965 年 由 美国 加 利 福 尼 亚 大 学 的 Zadeh 教授 提 
出 ,1974 年 英国 的 EE. H. Mamdani 成 功 地 将 模糊 控制 应 用 于 锅炉 和 蒸汽 机 控制 。 随 后 ,模糊 
控制 在 控制 领域 得 到 了 快速 发 展 , 并 获得 大 量 成 功 的 应 用 。 

模糊 控制 实质 上 是 一 种 非 线性 控制 ,属于 智能 控制 的 范畴 。 模 糊 控制 的 一 大 特点 是 既 有 
系统 化 的 理论 ,又 有 大 量 的 实际 应 用 背景 。 近 20 多 年 来 ,模糊 控制 不 论 在 理论 上 还 是 技术 上 
都 有 了 长 足 的 进步 ,成 为 自动 控制 领域 一 个 非常 活跃 而 又 硕果 累累 的 分 支 。 其 典型 应 用 涉及 
生产 和 生活 的 许多 方面 ,例如 家 用 电器 设备 中 的 模糊 洗衣 机 、 空 调和 吸尘器 等 ,工业 控制 领域 
中 的 水 净化 处 理 \ 发 酵 过 程 、 化 学 反应 签 等 ,在 专用 系统 和 其 他 方面 有 汽车 驾驶 、 电 梯 、 自 动 扶 
梯 以 及 机 器 人 的 模糊 控制 。 而 模糊 逻辑 在 机 器 人 中 的 应 用 ,可 能 会 使 机 器 人 变 得 更 具 独 特性 
和 智能 性 。 

5. 神经 网 络 控制 

神经 网 络 (Neural Network,NN) 控 制 是 基于 神经 网 络 的 控制 方法 ,简称 神经 控制 。 神 经 
网 络 控制 是 20 世纪 80 年 代 末 期 发 展 起 来 的 自动 控制 领域 的 前 沿 学 科 之 一 。 它 是 智能 控制 的 
一 个 新 的 分 支 ,为 解决 复杂 的 非 线性 、 不 确定 、 不 确 知 系统 的 控制 问题 开辟 了 新 途径 。 神 经 网 
络 控制 是 将 人 工 神经 网 络 与 控制 方法 相 结合 而 产生 的 一 种 智能 控制 方法 ,是 指 在 控制 系统 中 
采用 神经 网 络 这 一 工具 对 难以 精确 描述 的 复杂 非 线 性 对 象 或 进行 建 模 ,或 充当 控制 器 ,或 优化 
计算 ,或 进行 推理 ,或 故障 诊断 等 ,也 即 同时 兼 有 上 述 某 些 功 能 的 适应 组 合 ,将 这 样 的 系统 统称 
为 神经 网 络 的 控制 系统 ,将 这 种 控制 方式 称 为 神经 网 络 控 制 。 

在 控制 领域 ,将 具有 学 习 能 力 的 控制 系统 称 为 学 习 控 制 系统 ,属于 智能 控制 系统 。 因 为 神 
经 控制 具有 学 习 能 力 , 故 属于 学 习 控 制 ,是 智能 控制 的 一 个 分 支 。 神 经 控制 发 展 至 今 , 虽 仅 有 
十 余年 的 历史 ,已 有 了 多 种 控制 结构 ,如 神经 预测 控制 、 神 经 逆 系统 控制 等 。 


神经 网 络 控制 


CHAPTER 7 


随 着 被 控 对 象 越 来 越 复杂 ,人 们 对 控制 系统 的 要 求 也 越 来 越 高 。 由 于 传统 的 基于 模型 的 
控制 方法 需要 根据 被 控 对 象 的 数学 模型 及 控制 的 性 能 指标 来 设计 控制 器 ,具有 显 性 表达 知识 
的 特点 ,但 对 于 复杂 的 被 控 对 象 (如 模型 不 确定 .时 变 等 ) 和 环境 ,往往 难以 获得 其 数学 解析 式 。 
而 神经 网 络 控制 虽然 不 善于 显示 表达 知识 ,但 它 利 用 神经 网 络 强大 的 非 线性 逼近 映射 能 力 、 自 
适应 能 力 、 自 学 习 能 力 以 及 信息 综合 能 力 ,能 够 有 效 解决 复杂 的 非 线 性 ,不 确定 性 及 不 确 知 系 
统 的 模型 辨识 ,控制 和 优化 ,从 而 使 控制 系统 达到 优良 的 控制 性 能 。 此 外 ,神经 网 络 控制 还 可 
实现 对 机 器 人 动力 学 方程 中 未 知 部 分 精确 通 近 ,通过 在 线 建 模 和 前 馈 补偿 ,实现 机 器 人 的 高 精 
度 跟 踪 。 由 于 神经 网 络 控制 具有 上 述 优异 特性 ,在 控制 系统 中 的 应 用 也 比较 灵活 多 样 ,近年 来 


7.1 神经 网 络 控制 基本 原理 


7.1.1 神经 网 络 控制 的 基本 思想 

神经 网 络 用 于 控制 系统 设计 主要 针对 系统 的 非 线 性 、 不 确定 性 和 复杂 性 。 而 控制 系统 的 
目的 则 是 通过 输入 确定 的 适当 的 控制 量 , 使 系 
统 获得 期 望 的 输出 特性 ,如 图 7.1(a) 所 示 为 一 
般 反 馈 控制 系统 原理 图 。 图 7. 1(b) 中 用 神经 
网 络 取代 图 7. 1(a) 中 的 控制 器 并 完成 相同 的 
控制 任务 。 下 面 分 析 神 经 网 络 的 具体 工作 
过 程 。 

假设 某 一 控制 系统 ,其 输入 x 和 输出 > 满 


足以 下 非 线性 函数 关系 

y= Gb 
其 中 ,/(. ) 是 描述 系统 动态 的 非 线性 函数 
控制 器 的 任务 是 确定 最 优 的 控制 输入 量 ,使 OO 
得 控制 系统 的 实际 输出 y 能 够 完全 跟随 期 望 。 。 四 ”1 一 般 反 僻 控 制 系统 与 神经 网 络 
输出 ， 。 控制 系统 原理 图 


在 神经 网 络 控制 系统 中 ,神经 网 络 的 功能 是 对 输入 输出 进行 某 种 非 线性 映射 ,或 者 函数 变 
换 , 满 足以 下 关系 
u=g(yy) {752 
由 于 控制 系统 的 目的 是 实现 系统 输出 y 等 于 期 望 输出 ys ,将 式 (7.2) 代 入 式 (7.1), 可 得 
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y=fLgtys)] C7 
当 g(.)= 广 !(。) 成 立时 ,满足 控制 目的 , 即 > 一 yv。 

由 于 采用 神经 网 络 控制 的 控制 对 象 一 般 非 常 复杂 且 具 有 不 确定 因素 (如 外 部 扰动 等 ), 因 
此 函数 和 (。) 很 难 建立 。 这 时 ,利用 神经 网 络 逼 近 非 线性 函数 的 能 力 , 对 函数 f+ '(，) 的 输入 
输出 特性 进行 模拟 , 即 神经 网 络 控制 器 通过 不 断 学 习 实际 输出 y 与 期 望 输出 yj 之 间 的 差 值 
来 调整 神经 网 络 的 连接 权 值 ,至 误差 e 趋 近 于 零 , 如 下 

e 一 y7 一 yy 一 0 [| 
这 就 是 神经 网 络 模 拟 f/ "(，。) 的 过 程 。 可 以 看 出 ,神经 网 络 实现 控制 的 基本 思想 实际 上 就 是 
通过 其 学 习 算 法 实现 对 被 控 对 象 求 逆 的 过 程 。 

7.1.2 神经 网 络 控制 的 特点 

从 控制 的 角度 看 ,与 传统 的 控制 方法 相 比 ,神经 网 络 用 于 控制 的 优越 性 主要 有 以 下 几 点 。 

(1) 神经 网 络 具 有 很 强 的 自学 习 能 力 ,能 够 对 模型 不 确定 .不 确 知 的 过 程 或 系统 进行 有 效 
控制 ,使 控制 系统 能 够 达到 期 望 的 动 .静态 特性 。 

(2) 神经 网 络 采用 并 行 分 布 式 信息 处 理 , 具 有 很 强 的 容错 性 。 

(3) 神经 网 络 本 质 上 是 非 线 性 系统 ,可 实现 任意 非 线 性 映射 。 

(4) 神经 网 络 具 有 很 强 的 信息 综合 能 力 ,能 同时 处 理 大 量 不 同类 型 的 输入 ,并 能 很 好 地 解 
决 输入 信息 之 间 的 互补 性 和 元 余 性 问题 ,特别 适用 于 多 变量 系统 。 

基于 上 述 优 异 特性 ,神经 网 络 在 控制 系统 中 既 可 以 充当 对 象 的 模型 (如 在 有 精确 模型 的 控 
制 结构 中 ) ,控制 器 (如 反馈 控制 系统 ) ,也 可 以 在 传统 控制 系统 中 优化 计算 环节 。 另 外 ,将 神经 
网 络 与 专家 系统 、 模 糊 逻 辑 .遗传 算法 等 智能 控制 方法 或 算法 相 结合 ,可 构成 新 型 智能 控制 器 。 

7.1.3 神经 网 络 控制 的 分 类 

神经 网 络 的 结构 形式 较 多 ,分 类 标准 不 统一 ; 对 于 不 同 结构 的 神经 网 络 控制 系统 ,神经 网 
络 本 身 在 系统 中 的 位 置 和 功能 各 不 相同 ,学 习 方 法 也 不 尽 相 同 。 如 从 神经 网 络 与 传统 控制 和 
智能 控制 的 结合 方面 ,可 将 神经 网 络 控 制 分 为 基于 神经 网 络 的 智能 控制 和 基于 传统 控制 理论 
的 神经 控制 两 类 。 

基于 神经 网 络 的 智能 控制 即将 神经 网 络 与 其 他 智能 控制 方式 相 融 合 ,或 者 由 神经 网 络 单 
独 进行 控制 ,其 中 包括 神经 网 络 直接 反馈 控制 ,神经 网 络 专家 系统 控制 .神经 网 络 模 糊 逻 辑 控 
制 和 神经 网 络 滑 模 控制 等 。 

而 基于 传统 控制 理论 的 神经 控制 是 指 将 神经 网 络 作 为 传统 控制 系统 中 的 一 个 或 多 个 部 
分 ,用 来 充当 对 象 模型 估计 器 、 辨 识 器 ,控制 器 或 优化 计算 环节 等 。 基 于 这 种 方式 的 神经 控制 
有 很 多 ,有 神经 逆 动 态 控制 神经 自 适应 控制 ,神经 自 校正 控制 .神经 内 模 控 制 、 神 经 预测 nM 
神经 最 优 决策 控制 以 及 神经 自 适应 线性 控制 等 。 

由 于 神经 网 络 控制 的 方式 很 多 ,这 里 只 对 一 些 比较 常用 的 神经 网 络 控制 系统 进行 介绍 。 

1. 神经 网 络 监督 控制 

通过 对 传统 控制 器 进行 学 习 , 然 后 用 神经 网 络 控制 器 逐渐 取代 传统 控制 器 的 方法 , 称 为 神 
经 网 络 监督 控制 。 神 经 网 络 监督 控制 系统 的 结构 如 图 7. 2 所 示 。 

其 中 ,uj (t) 是 e(z) 的 函数 ,e(1) 是 y(t) 的 函数 ,y(t) 是 ult) 的 函数 ,而 u(t) 又 是 网 络 权 值 
的 函数 ,所 以 u, (1) 最 终 是 网 络 权 值 的 函数 。 因 此 可 通过 使 u(t) 逐渐 趋 于 0 来 调整 网 络 权 值 。 

当 w, (#) 二 0 时 ,从 前 馈 通路 看 ,有 : 

y= FLE KW = C7..5) 


第 7 章 神经 网 络 控制 | 因 77 


神经 网 络 |w(D 
控制 器 


图 7.2 神经 网 络 监督 控制 系统 的 结构 


此 时 再 从 反馈 回路 看 ,有 : e 二 yy 一 y= 二 0。 

神经 网 络 控制 器 实际 上 是 一 个 前 馈 控 制 器 , 它 建立 的 是 被 控 对 象 的 逆 模 型 。 神 经 网 络 控 
制 器 基于 传统 控制 器 的 输出 ,在线 学 习 并 调整 网 络 的 权 值 , 使 反馈 控制 输入 趋 近 于 零 , 从 而 使 
神经 网 络 控制 器 逐渐 在 控制 作用 中 占据 主导 地 位 ,最 终 取消 反馈 控制 器 的 作用 。 一 且 系 统 出 
现 干 扰 ,反馈 控制 器 重新 发 挥 作用 。 神 经 网 络 监督 控制 器 ,不 仅 能 够 确保 控制 系统 的 稳定 性 和 
鲁 棒 性 ,还 可 以 使 系统 的 精度 和 自 适 应 能 力 得 到 有 效 提高 。 

2. 神经 网 络 模型 参考 自 适 应 控制 

与 传统 自 适 应 控制 相同 ,神经 网 络 自 适应 控制 也 分 为 神经 网 络 自 校正 控制 和 神经 网 络 模 
型 参考 自 适应 控制 两 种 。 在 模型 参考 自 适应 控制 中 ,闭环 控制 系统 的 期 望 性 能 用 一 个 稳定 的 
参考 模型 来 描述 ,可 分 为 直接 模型 参考 自 适应 控制 和 间接 模型 参考 自 适应 控制 两 种 ,结构 分 别 
如 图 7. 3(a)、(b) 所 示 。 


ym(?) 


ec(D) 


人 


(b) 间接 模型 参考 自 适应 控制 
图 7.3 神经 网 络 模型 参考 自 适应 控制 系统 的 结构 
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如 图 7. 3(a) 所 示 为 神经 网 络 直接 模型 参考 自 适 应 控制 ,神经 网 络 控制 器 的 作用 是 使 被 控 
对 象 与 参考 模型 输出 之 差 为 最 小 ,从 而 使 被 控 对 象 的 实际 输出 > 跟踪 期 望 输出 > 。 

通过 使 e. 最 小 来 调节 神经 网 络 的 权 值 。 如 果 e. 王 0, 则 > 一 y。, 从 而 有 e 王 ~ 一 yw，e 作为 
神经 网 络 控制 器 的 输入 ,产生 控制 作用 。 


但 该 方法 需要 知道 对 象 的 数学 模型 (Jacobian 信息 辽 ) 才 能 通过 误差 反 向 传播 算法 修正 


网 络 权 值 ,但 对 象 通常 含有 未 知 参数 。 

如 图 7. 3(b) 所 示 为 神经 网 络 间接 模型 参考 自 适 应 控制 ,神经 网 络 辨 识 器 向 神经 网 络 控 制 
器 提供 对 象 的 Jacobian 信息 ,用 于 控制 器 的 学 习 。 

3. 神经 网 络 自 校正 控制 

自 校正 控制 根据 对 系统 正 向 或 逆 模 型 的 建 模 结果 ,直接 调节 神经 或 传统 控制 器 的 内 部 参数 ， 
使 系统 满足 给 定 的 指标 。 基 于 神经 网 络 的 自 校正 控制 也 分 为 直接 控制 和 间接 控制 两 种 类 型 。 

神经 网 络 直接 自 校正 控制 系统 由 一 个 神经 网 络 控制 器 和 一 个 可 进行 在 线 修正 的 神经 网 络 
辨识 器 组 成 ,调整 的 是 神经 网 络 控制 嚣 本身 的 参数 。 神 经 网 络 间 接 自 校正 控制 系统 由 一 个 常 
规 控制 器 和 一 个 具有 离线 辨识 能 力 的 神经 网 络 辨 识 器 组 成 ,神经 网 络 辨 识 和 市 作 入 各 少 玫 光 
某 些 非 线性 函数 的 在 线 估 计 器 ,神经 网 络 估计 器 需要 很 高 的 建 模 精 度 ,主要 用 来 调整 常规 控制 
器 的 参数 。 神 经 网 络 自 校正 控制 系统 ,结构 如 图 7.4 所 示 。 


图 7.4 神经 网 络 自 校 正 控制 系统 的 结构 


通常 假设 被 控 对 象 为 单 输入 单 输出 的 线性 系统 ,如 下 
y(R 十 1) 一 FLy(R)，…,y(R 一 2); ulk), ,ulk—m)] 
十 g[Ly(R)，…,y(R 一 2); xx) ulk—m)lulk) (nm) (7.6) 
其 中 ,wu(k) 为 控制 器 的 输出 ,y (十 1) 为 被 控 对 象 的 输出 。 
函数 f 和 8g 为 非 零 函数 , 当 这 两 个 函数 已 知 时 ,根据 “ 确定 性 等 价 原则 ”， 控制 器 可 以 采用 
如 下 控制 算法 : : 
Lt a 


芝 
使 控制 系统 的 输出 y(k 十 1) 能 精确 地 跟踪 系统 的 输入 r(k 十 1) , 即 期 望 输出 。 


当 函 数 上 和 gg 未 知 时 , 则 通过 神经 网 络 辨识 器 ,逐渐 台 近 被 控 对 象 , 即 由 辨识 器 的 广 和 8 
代替 函数 1 和 g ,重新 自 校正 控制 规律 。 为 了 简化 计算 ,假设 被 控 对 象 为 如 下 一 阶 系统 : 


u(k) = 


y(k+1)=f[y(k)]+ gLy(k)Julk) (C738) 
通过 神经 网 络 辨 识 器 利用 如 下 模型 去 允 近 被 控 对 象 模 型 : 
9 十 1) =fLy(k) ,wk)] HELy (Rk), vk) uk) (I 


其 中 ,w(k) 和 wl(k) 为 两 个 神经 网 络 的 权 系 数 , 它 们 分 别 为 
w(k) 二 [wo ywi(k) ,wo(k),…,w2s(k)] (zp 为 隐 含 节点 的 个 数 ) 
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v(k) 二 [vo ,vi(k),vs(k),…,vzo(k)] (9 为 隐 含 节点 的 个 数 ) 
且 有 f[0,w(k)]=wo ,$8[0,v(k)]=v,。 
此 时 ,相对 应 的 控制 律 为 
六 [>y(CE) mw(R)] r(& 十 1) 


a el 
将 式 (7.10) 代 入 式 (7.9) 可 得 
y+D=f[y(k)]+gLy(k)]. fly ,wk)] I (7.11) 


EEy RY VR » SLEy(k)svCE)] 


由 式 (7. 11) 可 知 , 当 且 仅 当 ff 和 8 一 g 时 ,才能 使 y(k 十 1) 一 r(k 十 1)。 

4. 神经 网 络 预测 控制 

预测 控制 又 称 为 基于 模型 的 控制 ,是 20 世纪 70 年 代 后 期 发 展 起 来 的 新 型 计算 机 控制 方 
法 ,该 方法 的 特征 是 预测 模型 .滚动 优化 和 反馈 校正 。 神 经 网 络 预测 控制 的 结构 如 图 7. 5 所 
示 ,神经 网 络 预测 器 建立 了 非 线性 被 控 对 象 的 预测 模型 ,并 可 在 线 进 行 学 习 修正 。 


图 7.5 神经 网 络 预测 控制 系统 的 结构 


利用 此 预测 模型 ,通过 设计 优化 性 能 指标 ,利用 非 线性 优化 器 可 求 出 优化 的 控制 作用 。 

5. 神经 网 络 内 模 正 控 制 

内 模 控 制 (也 称 内 部 模型 控制 ) ,该 算法 于 1982 年 由 Garcia 提出 ,因为 其 在 预测 控制 系统 
的 有 效 参 数 易 调 等 优点 ,很 快 在 控制 界 引起 极 大 关注 。 经 典 的 内 模 控制 将 被 控 系 统 的 正 向 模 
型 和 逆向 模型 直接 加 入 反馈 回路 ,系统 的 正 向 模型 作为 被 控 对 象 的 近似 模型 与 实际 对 象 并 联 ， 
两 者 输出 之 差 被 用 作 反馈 信号 ,该 反馈 信号 又 经 过 前 向 通道 的 滤波 器 (滤波 器 通常 为 线性 的 ) 
及 控制 器 进行 处 理 。 控 制 器 与 系统 的 逆 有 直接 关系 ,通过 引入 滤波 器 来 提高 系统 的 鲁 棒 性 。 

神经 网 络 内 模 控 制 系统 的 结构 如 图 7. 6 所 示 , 被 控 对 象 的 正 向 模型 及 控制 器 均 由 神经 网 
络 来 实现 。 实 线 为 基本 原理 图 ,加 上 虚线 后 可 构成 内 模 控制 的 一 种 具体 实现 。 


u(t) 2 JD 
神经 网 络 cz 
内 模 (辨识 ) ? 


图 7.6 神经 网 络 内 模 控制 系统 的 结构 
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7.2 未 知 动态 下 机 械 璧 全 局 神经 网 络 控制 器 设计 


针对 未 知 动态 条 件 下 的 机 器 人 系统 ,本 节 将 介绍 一 种 神经 网 络 控制 器 的 设计 方法 ,实现 全 
局 一 致 有 界 (Global Uniform Ultimate Boundedness，GUUB) 。 

机 器 人 技术 起 源 于 20 世纪 60 年 代 , 历 经 了 半 什 多 世纪 的 发 展 ,已 经 取得 了 丰硕 的 研究 成 
果 。 机 器 人 的 出 现 对 人 们 的 工作 方式 产生 了 深远 的 影响 , 它 可 以 代 蔡 人 类 在 恶劣 复杂、 危险 
的 环境 下 完成 工作 ,成 为 解放 人 类 劳动 力 不 可 或 缺 的 工具 。 机 器 人 在 各 领域 所 显示 出 的 突出 
优势 以 及 潜在 的 巨大 价值 ,使 得 机 器 人 技术 的 研究 受到 了 世界 各 国 的 重视 。 机 器 人 技术 的 发 
展 水 平 已 成 为 衡量 一 个 国家 工业 自动 化 程度 的 重要 指标 。 由 于 机 器 人 技术 在 社会 经 济 发 展 中 
起 着 重要 的 作用 ,其 需求 量 在 不 断 增 大 。 与 此 同时 ,对 机 器 人 控制 系统 需要 达到 的 各 项 动 、 静 
态 性 能 也 提出 了 更 高 的 要 求 。 

机 器 人 被 控 对 象 是 一 个 若干 关节 的 机 械 臂 ， 从 控制 工程 的 角度 来 看 ， 这 是 一 个 具有 非 线性 
和 不 确定 性 的 系统 。 机 器 人 在 工作 时 ,由 于 受到 建 模 与 测量 不 精准 ,系统 负载 随时 间 产 生 的 变 
动 和 外 界 干扰 等 因素 的 影响 ,因此 具有 较 强 的 时 变性 。 再 加 上 惯性 、 质 量 等 系统 参数 的 不 确定 
性 ,导致 了 机 器 人 系统 的 参数 不 确定 性 。 另 外 ,作业 环境 干扰 、 驱 动 器 饱和 问题 误差. 死 区 、 时 
滞 以 及 一 些 无 法 建 模 的 动态 问题 等 ,导致 机 器 人 系统 参数 的 不 确定 性 。 通 常情 况 下 ,很 难 建 立 
精准 的 动力 学 模型 ,因此 基于 精准 系统 数学 模型 的 反馈 控制 在 实际 中 无 法 满足 高 精度 和 高 品 
质 的 控制 要 求 。 所 以 机 器 人 控制 中 的 一 个 关键 问题 在 于 如 何 应 对 机 械 辟 动态 系统 的 不 确定 
因素 。 


7.2.1 背景 

1. 机 械 辟 控制 技术 

因为 机 械 臂 具有 多 个 关节 ,分 别 需要 相应 的 驱动 电机 提供 驱动 力矩 ,并 输出 相应 的 位 置 、 
速度 和 加 速度 ,所 以 机 械 臂 的 控制 是 一 个 多 输入 多 输出 的 问题 。 

机 器 人 技术 在 几 十 年 的 发 展 历程 中 ,积累 了 丰富 的 控制 理论 应 用 的 相关 经 验 。 到 目前 为 
止 ,很 多 控制 算法 已 成 功 地 应 用 于 机 器 人 控制 ,并 取得 了 显著 的 成 果 。 如 PID 控制 , 它 是 机 器 . 
人 系统 中 非常 经 典 的 控制 算法 ,因为 其 具有 控制 律 简单 , 调 参 经 验 丰富 ,物理 实现 简单 而 且 无 
须 建 模 的 优点 。 但 是 这 种 控制 算法 无 法 保证 系统 的 动静 态 特 性 ,所 以 对 于 具有 强 非 线性 和 强 
不 确定 性 的 日 益 复杂 的 系统 ,开发 更 强大 的 控制 算法 是 十 分 必要 的 。 为 了 提高 控制 性 能 ,许多 
控制 方法 已 经 发 展 到 考虑 如 何 应 对 机 械 臂 动态 系统 的 不 确定 因素 阶段 ,如 滑 模 变 结构 控制 、 鲁 
棒 控 制 . 自 适应 控制 等 更 高 级 的 控制 算法 已 经 出 现 。 在 机 器 人 控制 算法 设计 中 ,已 经 有 越 来 越 - 
多 的 学 者 将 目光 转移 到 智能 控制 理论 。 相 比 于 经 典 控制 理论 和 现代 控制 理论 ,智能 控制 具有 
较 好 的 学 习性 能 。 作 为 一 种 新 型 的 控制 方法 ,将 它 应 用 到 机 器 人 控制 系统 还 需要 更 深入 的 
研究 。 

2. 机 械 臂 神经 网 络 控制 技术 

由 于 机 器 人 系统 的 复杂 性 ,关于 机 器 人 动力 学 参数 的 先 验 知识 可 在 实际 应 用 中 使 用 得 很 
少 。 基 于 对 象 模型 的 经 典 控制 方法 已 经 越 来 越 难以 满足 机 器 人 控制 的 要 求 。 因 此 ,在 机 器 人 
控制 技术 的 发 展 过 程 中 ,出 于 对 更 高 控制 品质 的 追求 ,智能 控制 理论 成 为 发 展 机 器 人 控制 技术 
生生 梁 类 六 月 之 一 。 着 吉 光 入 网 二 当 入 机 玫 和 各 由 负 坟 济 太 于 新 部 交锋 力 、 特 禄 并 癌 划 技术 

结合 到 机 器 人 控制 中 ,可 以 解决 机 器 人 模型 复杂 的 问题 。 神 经 网 络 具 有 万 能 逼近 的 特性 ,能 够 
学 习 机 器 人 系统 的 未 知 动态 系统 参数 ,并 能 够 通过 自身 的 拓扑 结构 映射 出 这 种 非 线性 表达 式 。 
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因为 神经 网 络 的 建立 过 程 不 依赖 于 机 器 人 的 实际 参数 ,所 以 它 建 立 的 非 线 性 关系 能 够 使 系统 
避免 参数 不 确定 性 对 控制 造成 的 影响 ,进而 真正 实现 智能 化 应 用 。 

目前 对 神经 网 络 的 研究 和 应 用 有 很 多 。 有 的 学 者 采用 一 种 基于 函数 和 逼 近 技术 的 自 适应 阻 
抗力 控制 器 并 将 其 应 用 到 双 机 械 臂 从 动 臂 的 运动 控制 中 ,取得 了 较 好 的 控制 效果 。 也 有 学 者 
使 用 神经 网 络 控制 器 来 通 近 未 知 函 数 。 针 对 严格 反馈 形式 的 离散 时 间 系 统 , 有 学 者 设计 了 结 
合 Backstepping 方法 的 自 适应 神经 网 络 控制 技术 ; 也 有 学 者 将 神经 网 络 用 来 估计 控制 增益 。 
当前 ,也 有 很 多 学 者 对 具有 时 间 延 迟 的 严格 反馈 系统 进行 了 深入 研究 。 对 于 柔性 关节 机 器 人 
的 轨迹 跟踪 问题 ,有 学 者 提出 基于 神经 网 络 控制 设计 方法 实现 系统 跟踪 误差 最 终 一 致 有 界 。 
针对 带 有 不 确定 性 的 柔性 关节 机 器 人 系统 ,有 学 者 提出 一 种 自 适应 神经 网 络 动态 面 控制 ,得 到 
_ 闭环 系统 所 有 信号 半 全 局 一 致 有 界 。 

然而 ,必须 要 注意 的 是 ,只 有 在 盘 近 区 域 的 紧 集 内 ,神经 网 络 的 逼近 能 力 才 是 有 效 的 。 因 
此 ,基于 神经 网 络 的 机 械 臂 控制 器 只 能 保证 半 全 局 一 臻 最 终 有 界 (Semi-Global Uniformly 
Ultimately Bounded,SGUUB) 稳 定性 。 需 要 指出 的 是 ,上 述 智能 设计 是 在 逼近 能 力 总 是 保持 
有 效 的 前 提 条 件 下 进行 的 ,这 一 先 验 条 件 是 很 苛刻 的 。 因 此 ,迫切 需要 发 展 能 保证 全 局 一 臻 最 
终 有 界 的 控制 器 。 为 了 弥补 神经 网 络 通 近 区 域 的 局 限 性 ,在 控制 器 里 面 附加 一 个 额外 的 鲁 棒 
控制 器 是 一 种 非常 有 效 的 解决 方法 。 

本 节 将 研究 间接 神经 网 络 动态 稳定 控制 系统 设计 ,结合 全 局 神经 网 络 控制 器 和 和 鲁 棒 控制 
器 来 实现 全 局 一 致 最 终 有 界 (GUUB)。 和 上 述 基 于 先 验 条 件 的 设计 方法 相 比 ,加 入 了 动态 面 
控制 (Dynamic Surface Cortrol, DSC) 设 计 , 所 采用 的 切换 信号 不 需要 nn 阶 光滑 ,并 且 大 大 放松 
了 传统 神经 网 络 近 似 设计 相关 的 约束 。 闭 环 系 统 的 全 局 一 致 最 终 有 界 (GUUB) 可 由 李 雅 普 诺 
夫 方法 严格 证 明 。 


7.2.2 全 局 自 适 应 神经 网 络 控制 器 设计 


1. 机 械 禹 控制 系统 
本 节 所 研究 的 机 械 臂 闭环 控制 系统 整体 结构 图 如 图 7.7 所 示 。 


图 7.7 机 器 臂 闭 环 控制 系统 整体 结构 图 


下 面 将 对 机 械 臂 闭环 控制 系统 结构 图 进行 分 析 。 在 基于 神经 网 络 的 机 械 臂 控制 系统 中 ， 
先 在 任务 空间 / 笛 卡 儿 空 间 指定 参考 轨迹 zx, ,然后 将 其 转化 为 运用 闭环 道 运动 学 (Closed 
Loop Inverse Kinematics,CLIK) 方 法 计算 给 定 关节 和 角 的 过 程 中 生成 的 q, 。 其 算法 如 下 : 
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gy =| KpJ (gede (7.12) 


其 中 ,Kp 是 正定 矩阵 ,J(q) 是 雅 可 比 矩 阵 ,e 是 机 械 臂 在 任务 空间 / 笛 卡 儿 空间 的 实际 位 置 x 
与 任务 空间 / 笛 卡 儿 空 间 指定 参考 轨迹 zy 的 差 值 。 
2. 全 局 自 适应 神经 网 络 控制 器 设计 
根据 前 面 的 分 析 可 知 ,n 关节 的 机 械 臂 的 动力 学 方程 有 如 下 形式 : 
M(g)gi+C(g,90)9+G(g)=r 《7. 汪 3 
且 具 有 如 下 性 质 : 
性 质 1: 惯性 矩 M(gq) 是 一 个 对 称 正定 矩阵 。 
性 质 2: M(g) 十 C(g ,9) 是 斜 对 称 和 矩阵 。 对 于 VzER” ,满足 z (M(g) 十 C(q,9))z=0。 
性 质 3: M(g)、C(g,9)、G(g) 均 有 界 。 
假设 : 系统 中 所 有 的 参考 信号 及 其 导数 都 是 光滑 且 有 界 的 函数 。 
将 动力 学 方程 转化 为 严格 反馈 形式 下 的 多 输入 多 输出 (MIMO) 向 量 函 数 表 达 式 : 
这 
(7. 14) 
zi, =—M "(Crys+G)+M iu 
其 中 ,zx1 一 q 二 [gi;*…,q,]ER" ,zz 一 q 一 [gi1,…,qn]€R"。 假设 M(g)、C(q,4)g、G(gq) 都 
是 未 知 的 。 定 义 3i=z1,zs 二 [x13X2 ]: 53 并且 令 一 MY” (GzztO) 计 Pz M =H(ZD; 
基于 Backstepping 方法 和 动态 面 技术 的 控制 器 设计 共 分 为 两 步 ,在 控制 器 的 设计 中 ， 
RBF 神经 网 络 用 来 通 近 未 知 的 动力 学 模型 一 M (Cx, 十 G)。 
步骤 1: 机 械 辟 关节 角 误 差 定 义 为 二 zx1 一 +14 ,其 中 zi 二 qa 是 关节 角 给 定 值 。 对 1 


求 导 可 得 : 
| 一 tg 一 工 ? 一 之 1 (C7: 15) 
其 中 ,jz 一 Cu。 
将 x 作为 式 (7. 15) 的 虚拟 控制 器 ,并 设计 zx,. 表达 式 如 下 : 
Zi =— Kizi tig CR LG 


其 中 ,Kl 二 diag(k11,k1so，…,k1,),; 且 有 1; 记 0,i= 二 1,2,…,n。 
定义 ,二 zs 一 zz4 ,一 个 新 的 误差 变量 ys 满足 : 
Ve CE 
式 (7.15) 可 以 转化 为 如 下 形式 : 
主 | 王 交 了 谤 向 
=Zs Xa rod — Tae To tla 
= 二 35 CO— Kz 《9 
步骤 2: 机械 臂 关 节 角 速度 误差 信号 定义 为 却 * 王 zy 一 zy ,其 中 zy 一 ay。 对 工 , 求 导 ， 
并 代入 式 (7. 14) 可 得 : 


其 中 ,zz 一 gu。 
使 用 RBF 神经 网 络 在 紧 集 03, 内 通 近 未 知 的 动态 模型 下 ,可 得 : 
F=Wi'Sp (7) 十 er (7. 20) 


其 中 ; 是 理想 的 权 值 矩阵 ,Sr 是 基 郴 数 向 量 ,er 是 误差 向 量 , 同 时 满足 | EF | <~e,, o 
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Ar=—K,z, 十 ty 一 By (ED)aN — (I —B,(z,))u’ J 
其 中 ,K ,二 diag(ky，… ,2,) ,ky 之 0,i 二 1,…,n,IT 是 单位 矩阵 ,同时 从 = 多 W5S4 ,并 有 如 下 
等 式 成 立 : 
uv =F(z,) =WISE(z,) 
@ -pranh (EE 2) I 
其 中 ,WE 和 六 分别 是 对 Wi 和 Wi7 的 估计 ,ws 是 一 个 设计 的 正 参数 ,FV (5,) 是 未 知 动 
态 下 (zs,) 的 上 界 。 式 (7.19) 可 写 为 : 
tz, =F+Hr Ss 
一 ( 互 十 站)r 十 下 一 
=Hr— K,z,+B(z)(F+ep)+ (I—B,(z))(F—u’) 2) 
其 中 ,下 一 全 TS (zx,),H=WiSyp (z2) ,Ws =Wi C—Ws ,Wp =W Ww, 。 
全 局 神经 网 络 的 自 适应 学 习 算法 如 下 : 
序 。 =Tp (Spz1B, (zs) — OrWe) 


4 (7. 24) 


其 中 ,Pr 和 了 Pr 是 设计 的 正定 矩阵 ,Sr 和 5p 是 设计 的 正 参数 。 

至 此 ,全 局 自 适 应 神经 网 络 的 设计 已 经 完成 ,其 基本 思想 是 在 传统 自 适 神 经 网 络 控制 器 的 
基础 之 上 通过 引入 一 个 阶 导 光 滑 的 切换 函数 B,(z,), 将 神经 网 络 控制 器 与 鲁 棒 控制 器 相 
结合 ,它们 共同 作用 ,实现 系统 全 局 一 致 最 终 有 界 (GUUB) 稳 定 。 

对 控制 器 工作 原理 的 分 析 如 下 。 

在 式 (7. 16) 和 式 (7. 21) 中 ,对 于 KK;z; (一 1,2) 提 供 了 误差 反馈 和 切换 函数 B,(z,)、 神 经 
网 络 函 数 逼 近 项 xxX 和 和 鲁 棒 项 w" ,它们 相互 配合 实现 全 局 跟踪 。 

以 式 (7. 20) 为 例 , 如 果 |zi; | 过!r;,; ; 则 zj; 此 时 在 区 域 ; ,2, 内 ，| 多 R 
因此 是 uN 工作 ,如 果 |zu | 之 7; , 则 zi 在 区 域 : ,0， 外 部 ,此 
时 wu” 工作 将 状态 变量 拉 回 到 区 域 ',Q， 内。 如 果 !1r;， 过 
|zi | 入 ji ; 则 B,C(z2)un 一 (I 一 B, (Es))u 将 状态 变量 拉 回 
到 区 域 。 切 换 函 数 B,(z,) 使 得 神经 网 络 控制 器 和 重 棱 控制 器 结 
合 在 一 起 工作 ,保证 系统 的 全 局 一 致 最 终 有 界 稳定 。 如 图 7.8 所 ”图 7.8 状态 变量 轨迹 图 
示 为 状态 变量 轨迹 图 。 
3. 神经 网 络 收敛 性 及 稳定 性 分 析 


选取 如 下 李 雅 普 诺 夫 函 数 : 
V 三 Vi 十 V， (7255 
其 中 : 
" = (Tz 二 ylyy 
Ee 1 ee Po 7. 26) 
lv = Fz2F2 toir WeTp We +WHTp We) 


根据 y, 在 式 (7.16) 中 的 定义 ,可 得 : 
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2 = 一 让; 二 a (一 ys) 十 D,(*) C7279 
其 中 D,(。)== 一 之 ,.。 
根据 虚拟 控制 器 的 定义 即 式 (7. 16) 以 及 假设 1, 可 知 存 在 DI 这 0, 并且 满足 : 
| Wt SD (7. 28) 
对 李 雅 普 诺 夫 函 数 Vi 求 导 可 得 : 
Vi 一 二 1 天 十 y29 
=X1 1 (元 Ky) 十 32 Ce  (—y2) Dy 
=Z1 (— KI1)z) 十 去 1 元 十 到 1 yy — yla OA 
全 


1 
zi1(— Ki)zi 二 一 


2 Es | 十 到 1ys 1 一 


|z1 12? 十 十 去 | 
时 1 
Ba 一 ya) 十 广 1y7 | 十 志 (DY) 
5 1 1 
<—izi1(KI— Di— yi(a a EE a 


i 二 [| (7. 29) 
其 中 ,y= 二 min[Amin(2C(K1 一 1)),Amin(2(a 1—1))],Kis=1/2(D2) 
对 李 雅 普 诺 夫 函数 V， 求 导 可 得 : 
V, 三 5 二 Fi tr WwW, FNriw, 
=z1(Hr— K,z,+B,(z)(F+ep)t+ (I—B,(z))(F—u’))— 
tr (WT FRIW 十 W rz ) (7. 30) 
将 神经 网 络 的 自 适应 学 习 算法 式 (7. 23) 代 入 式 (7. 30) ,可 得 : 
Vs, =z1z, — tr (WITE) 凤 ; 十 到 下 工地 W,,) 
=z!lHr—ilK,r, +i! B,(z)F i B, (zo)er zl (I —B,(z))(F—u’)— 
tr (WESEzlB, (zis) — pWIWe) — tr (WHSHril — SnWhWa) 
=—zlK,z, +z!1B,(z)er +i1 (I — B,(z,))(F—u’)+ 


tr(OreWIiWe +onWiWa) C7 1 
根据 上 述 内 容 可 知 有 如 下 不 等 式 成 立 : 


tr (WIW:) <— Ftr (WEW:) 十 Ftr (Ww? TWi) 


tr (WEiWa) <— 7 tr(WiWa) 十 二 地 tr (WHT WE ) 
(2 


上 


| 


5B;(E)er 魏 二 | 无 | 


[Se 


zl(F—u’) <ilF 2zIFVtanh (= 
WwW 
利用 上 述 式 (7. 32) 可 得 : 
V, =—zlK,z, iliB,(zo)er tri (I — B,(z,))(F—u’)+ 


) 入 mu 
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有 1 一 下 ,全 一 下 和 ， 
-zs Kuti 十 | 元 2 bs 十 kw 十 杂 (OFTWTHWr +épWi Wy) 


<—i2(K,— 1)z, -了 |z1 12 一 Ftr (Op WEW: +6pgWHWp) 十 


1 其 并 六 其 
Fir (FWF TWi +OFWi Wi) + Fe 十 ks 
1 
<— NV+ lz | + Ks (7. 33) 


其 中 a min[ NE/ tI DA Ls OhE=i RIR: =7|S05 同 
时 K,s=1/2tr (GFWE Wi +OngWi Wi)+1/2e 十 kwy 。 

综合 上 述 分 析 可 得 : 

VV +Y 近 一 人 7 十 天 (7. 34) 

其 中 ,yy 二 min[w1 ;wsj ,同时 K 二 Ks 十 Ks。 

根据 Lasalle-Yoshizawa 定理 可 知 ,系统 是 稳定 的 。 令 my 二 K/P,, 可 以 得 到 V=P。 时 
V 二 0。 因 此 ,如 果 V(0) 志 P, , 即 对 于 任意 1 这 0, 有 V() 达 P。 成 立 。 通 过 解 以 上 不 等 式 (7. 34) 
可 得 : 


oSVD ST+(VO -5)e, Vi 二 0 CS 


经 上 述 分 析 可 知 ,可 以 通过 增 大 控制 增益 
(Ki ,K，) 和 减 小 一 阶 滤波 系数 a 的 方法 ,使 得 系 
统 的 跟踪 误差 任意 小 。 

7.2.3 仿真 实验 


1. 模型 描述 
仿真 实验 以 两 自由 度 机 械 臂 为 研究 对 象 。 如 
图 7. 9 所 示 ,其 动力 学 方程 为 : 


be | a 后 | a 济 
q gq = 
Ma Ms2? Vy V2», Cs 
其 参数 为 ， 图 7.9 两 自由 度 机 器 人 结构 
Mi 一 mi1225 十 ma(0 十 22 十 2017.cosgy) 十 TI) 十 T， 
Miy 一 my (Lislilscosgs) + I, 


Mi 一 my (02 十 10cosqy) 十 T， 
M ,, =m2li, dy 


Ci =—m2lil.292sing, 

Ciz =—m2liles (91 十 0z)singz 

Cal =—m2liled sing, 

Co 一 0 

Cl1 王 (all 十 72Li1)gcosql 十 Mgcos(ql1 十 qy) 
G, =m,l.sgcos(gi 十 qy) 


第 1 个 和 第 2 个 连 杆 的 质量 分 别 为 mi 一 2kg 和 m, 二 0. 85kg, 两 个 关节 的 长 度 分 别 为 /= 
0.35m 和 /7, 二 0.31m。 了 i 二 0.06lkgm* 和 了 ,二 0.020kgm’ 分 别 为 第 一 个 和 第 二 个 关节 的 惯 
量 矩 阵 。 
两 自由 度 机 械 臂 的 参考 轨迹 分 别 设 定 为 qu 二 sint,qas 王 2cost, 其 中 t+€EL0,tyj], 并 且 
ti 一 20s。 控 制 增益 设 为 玉 ; 一 diag(8,8) ,KK 一 diag(8,8)。 在 全 局 自 适应 神经 网 络 学 习 算法 
中 ,参数 设置 为 Pr = 二 101 ,Ty 王 107,9r 一 0.001,87 一 0.001。 神 经 网 络 的 节点 数 设 置 为 N = 
256 , 鲁 棒 控 制 器 w” 的 参数 设计 为 w= 二 0.01, 切 换 函 数 的 边界 分 别 选 为 'r; ,二 1.5,r;,, 二 2.5 
并 且 届 ==1,5 二 1 

2. 仿真 结果 与 分 析 

两 自由 度 机 械 辟 的 参考 轨迹 分 别 设 定 为 qu 二 sint 和 gs 二 2cost 时 ,可 以 得 到 关节 角度 
qi 和 gq 的 跟踪 效果 如 图 7.10、 图 7.11 所 示 , 而 关节 角速度 Y， 和 gs, 的 跟踪 效果 如 图 7. 12、 
图 7. 13 所 示 。 


一 一 关节 角度 qi 
-~~ 参考 轨迹 qu 


关节 角度 qi/rad 


‘1 
时 间 /sec 


图 7.10 关节 角 ai 的 跟踪 效果 图 


一 一 关节 角速度 ql 
~ 参考 角速度 da 


一 一 关节 角度 9 
~~~ 参考 轨迹 gm 


关节 角度 qy/rad 


0 2 4 fr 1 14 16-18.20 
时 间 /sec 


图 7.11 关节 角 g, 的 跟踪 效果 图 


一 一 关节 角速度 4 
-一 ~ 参考 角速度 gp 


关节 角速度 qi/rad/s 
关节 角速度 qs/rad/s 


0 2 4 6 % 012 16-18 20 9 未 
时 间 /sec 时 间 /sec 


图 7.12 关节 角速度 4 | 的 跟踪 效果 图 图 7.13 关节 角速度 9, 的 跟踪 效果 图 


关节 控制 力矩 如 图 7. 14 所 示 。 根 据 仿真 结果 可 以 看 出 ,在 动力 学 参数 未 知 的 情况 下 ,本 
节 设 计 的 全 局 自 适应 神经 网 络 控制 器 能 够 使 系统 的 关节 角度 和 角 加 速度 跟踪 给 定 的 轨迹 。 在 
系统 运行 的 初始 阶段 ,跟踪 轨迹 存在 较 大 偏差 ,这 是 因为 神经 网 络 的 权 值 范 数 ( 如 图 7. 15 所 
示 ) 还 没有 通过 学 习 算 法 调整 到 最 优 。 随 着 神经 网 络 学 习 算法 学 习 过 程 的 推进 , 权 值 矩 阵 迅 速 
收敛 到 最 优 值 。 神 经 网 络 自 适应 学 习 算法 在 本 质 上 是 一 种 梯度 下 降 方法 ,能 够 很 好 地 通 近 未 
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知 动态 函数 。 神 经 网 络 的 学 习 速度 很 快 ,大 约 用 时 1s。 因 此 ,通过 仿真 结果 可 以 看 到 ,全 局 自 
适应 神经 网 络 控制 器 的 轨迹 跟踪 效果 较 好 ,从 而 其 有 效 性 得 到 验证 。 当 然 , 所 设计 的 控制 器 不 
是 只 针对 两 自由 度 机 械 臂 有 效 , 也 可 以 将 其 拓展 到 ”自由 度 。 


25 2 
20 1 
0 
15 
E 有 一 下 
& 10 < 
~ 全 -人 
5 
3 
0 -4 
一 he 


人 2 6 50 Wl 4 .15 118. 20 
时 间 /sec Mb 
(a) 关节 控制 力矩 m (b) 关节 控制 力矩 世 


图 7.14 关节 控制 力矩 


0.7 0.7 
0.6 0.6 
0.5 0.5 
= 0:4 二 0.4 
E SS 
03 03 
02 0.2 
0.1 0.1 
0 00 


i222， 届 个 3° 0 lz 14 16 18- 0 10 1 14 16 ~ 18 "20 
时 间 /sec ce 
(a) 权 值 范 数 丙 (b) 权 值 范 数 到 


7.15 权 值 范 数 


7.3 一 种 具有 输出 约束 的 机 械 臂 轨迹 跟踪 神经 网 络 控制 


在 机 器 人 应 用 中 ,机 器 人 控制 需要 满足 某 些 约束 条 件 , 例 如 位 置 .速度 以 及 加 速度 等 。 这 
些 约束 条 件 可 能 导致 系统 不 稳定 甚至 损坏 。 因 此 ,在 设计 机 器 人 控制 器 时 ,考虑 这 些 约束 至 关 
重要 。 障 碍 李 雅 普 诺 夫 函 数 (Barrier Lyapunov Function,BLF) 是 现 有 的 处 理 约束 效应 的 方法 
之 一 ,可 以 保证 满足 约束 。 

通常 ,基于 BLF 设计 的 控制 器 包含 不 确定 项 。 现 有 的 逼近 器 有 很 多 种 类 型 ,都 可 以 减少 
通 近 误差 ,如 自 适应 模糊 迎 辑 系统 。 在 这 些 允 近 器 中 ,神经 网 络 的 通 近 能 力 较 强 。 而 基于 自 适 
应 通 近 的 控制 方法 在 处 理 不 确定 性 问题 时 表现 了 强大 性 能 ,因此 常 将 神经 网 络 加 入 到 控制 器 
中 ,以 消除 不 确定 性 。 

本 节 使 用 径 向 基 函 数 神经 网 络 (RBF-NN) 作 为 前 馈 预 测 器 来 通 近 机 械 臂 的 未 知 动力 学 。 
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对 于 有 m 个 中 心 的 n 连 杆 机 械 臂 ,传统 自 适 应 逼近 控制 器 的 输入 数量 是 5n (包括 3n 预定 的 
参考 轨迹 和 2n 跟踪 误差 ) 。RBF-NN 的 节点 个 数 是 m 的 5n 次 方 。 而 对 于 前 馈 - 反 馈 混 合 自 
适应 逼近 控制 (HFF-ACC) ,RBF-NN 的 节点 是 m 的 3n 次 方 ( 仅 利 用 所 需 的 输出 ) 。 显 然 , 输 
人 数量 减少 ,神经 网 络 中 的 节点 数量 将 相应 地 大 大 减少 。 

本 节 介 绍 一 种 简化 的 RBF-NN 控制 方法 , 它 利 用 系统 的 期 望 输出 作为 神经 网 络 的 输入 ， 
输入 到 基于 tan-type 型 障碍 李 雅 普 诺 夫 函 数 设计 的 约束 控制 器 中 。 

7.3.1 机 械 句 数学 模型 

1. 系统 描述 

对 于 具有 nn 自由 度 (DOF) 的 机 械 臂 ,关节 空间 的 动力 学 可 以 建立 如 下 : 

Mo)g 十 C(a0)6 十 G(Go) 王 rz 十 rv (TB 

其 中 ,g ER”"*” 是 关节 空间 中 的 配置 ,M (g)ERR”"” 为 对 称 正定 惯性 矩阵 ,C(g ,9)ERR"”” 表 
示 科 里 奥 利 和 离心 矩阵 ,G(q)ER ”为 重力 矢量 扭矩 ,rzE 了 ”和 rv ER ” 分别 表示 控制 的 扭矩 
和 由 于 未 知 干 扰 力 引起 的 扭矩 。 机 械 臂 有 下 面 两 个 性 质 。 

性 质 1: M(g) 一 C(g,9) 是 反对 称 的 , 即 &T(M(g) 一 C(g,9))&==0,V EER"。 

性 质 2: M(g)、C(g,9) 和 G(g) 的 一 阶 导数 可 微 , Vg,g ER”。 

控制 目标 是 在 输出 约束 和 未 知 干扰 条 件 下 ,跟踪 参考 轨迹 。 系 统 状态 g 二 [gq1 ,qs，…， 
gq, 」! 的 约束 为 

las Ne Rs VO tl) 

其 中 , ;>0 为 常数 。 输 出 约束 定义 为 6: = 一 {g ER” ||g;|<k;, Vt>0}CR” ,i=1,2,"…,n 

2. 障碍 李 雅 普 诺 夫 函数 (BLF) 

BLF 广泛 应 用 于 柔性 系统 。 这 里 采用 BLF 来 解决 约束 问题 。 选 择 一 个 改良 的 tan 型 障 
碍 李 雅 普 诺 夫 函 数 如 下 : 


k? née? 
V » 0 k Ti38 
SA 三 tan (Ts ) | 有 50 到 ( ) 


其 中 , 表示 机 械 臂 的 状态 ,& 是 用 户 定义 的 大 于 零 的 常数 约束 。 
3. 径 向 基 函 数 神经 网 络 (RBFNN) 
RBFNN 是 一 种 有 效 的 前 馈 神经 网 络 。 采 用 线性 参数 化 RBF-NN 来 近似 未 知 函 数 H(zx)E 
及 " ,x ER 及”。RBF-NN 表示 如 下 : 
H(z)=®'(z)0 (7. 39) 
其 中 ,0: ==col (wi yw wj) ER” ,m=n el,w,= [ww wi ER ,i= 1 
1,B ;二 [$1 (x) ,go (rT) ,$1 (z)]ER'YY1(i=1,2,.… ,nn),!l 是 RBF-NN 中 的 节点 数 。 选 择 
$;《X) (i 二 1,2,… ,1) 为 高 斯 函数 ,如 下 : 
Ne 
4 -em 人 上 地) 
其 中 ,cER” 和 o; ER ”分别 是 RBF 的 中 心 和 宽度 。 
4. 引 理 和 假设 
本 节 将 基于 以 下 引 理 和 假设 对 控制 器 进行 设计 。 
引 理 1: 在 V(0) 是 有 界 的 初始 条 件 下 , 李 雅 普 诺 夫 候选 函数 V(t) 是 连续 可 微 的 。 
V<—pV+c (7. 41) 
其 中 ,o>0,c>0。 解 xz (1) 是 均匀 有 界 的 。 


(7.40) 


第 7 章 ”神经 网 络 控制 |l 萝 89 


引 理 2; 假设 有 函数 y; (zx;)==$; (zx)w; 十 e; (i 二 1,2,…,1), 其 中 ,e; 为 近似 误差 , 且 e; 有 
界 ,满足 maxseo 上 ;1 <a;。 其 中 ,e; 是 未 知 边界 。 

假设 : r， 有 界 且 受 条 件 |r, (i)| 入 zy 约束 ,其 中 ,tz ERR1+ 是 未 知 常数 。 

7.3.2 控制 器 设计 

1. 基于 模型 的 控制 器 

本 节 将 对 n 自由 度 机 械 辟 , 在 输出 约束 条 件 |&1; | 过 &; , (i 二 1,2,…,n) 下 ,设计 一 个 关节 
空间 控制 器 。 该 机 械 臂 的 动力 学 模型 可 以 在 关节 空间 中 表示 为 : 


所 一 工 ? 
zi, =M '(g)[r—ry—C(g,9)9 —G(g)] (7342) 
入 本 
其 中 ,zi: 一 qzy: 一 0。 
假设 误差 信号 为 : 
é1 = CT, AA3) 
a | (7. 44) 


其 中 ,8; 是 将 要 设计 的 控制 器 的 未 知 虚拟 输入 。 
6 的 误差 动力 学 可 写 为 : 
Mé, 十 Ce 一 rr 一 MP 一 C8 一 G 十 r， (7. 45) 
其 中 ,M、C、G 分 别 是 M(g)、C(g,9)、G(g) 的 缩写 。 
考虑 约束 |&1; | 二 ,i 二 1,2,…,n ,选择 tan 型 障碍 李 雅 普 诺 夫 函数 为 : 


. 六 nél; 1 .7 
Vi()= 2») 到 tan 人 )+ 5 Me, (7. 46) 


i 一 1 
对 式 (7.43) 和 式 (7. 46) 求 导 ,与 式 (7. 44) 和 式 (7. 45) 结 合 ,可 得 ， 


. €1;(€2; 十 EF ; 7 
VC 一 >， Sa Th te) | or Me, 十 C8&,) 十 EI C(t 十 ty 一 (MA 十 C8 十 G)) 


= 3t 
全 
C7. 47) 
基于 7.3.1 节 的 引 理 1, 将 式 (7.47) 写 成 式 (7. 41) 的 形式 ,可 以 设计 参数 B| 和 + 如 下 : 
ki E E23 区 = 
I 和风 和 是 = 
S117 2 2 和 
Bi 一 一 开 : (7. 48) 
i Ee 人 3 
S se 
Eln™ 2K2 242， 
é11 
(| 
COS 
242) 
ri 一 一 : 一 天 二 人 本 Mi 十 Go +tG (7. 49) 
二 
:人 
COS 2 
2k 
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选择 KK 二 diag(k ji Rom) 为 常数 对 角 矩 阵 。 将 式 (7.48) 和 式 (7. 49) 代 入 式 (7.47)7 有 : 


ne ; 

= Dh ton(mY) -Ks <—pVi ln) 07. 50) 

, . DA CR YN ， 
其 中 ,pi 大 于 0, 且 满足 约束 条 件 Pi 一 min (一 GG 一 1,2,… wn)。 因 此 ,C4) 负 定 ， 


max 


即 闭环 系统 是 稳定 的 。 
2. 设计 动力 学 参数 未 知 的 控制 器 
由 于 M、C、G 的 不 确定 性 , 式 (7.49) 的 控制 器 无 法 实现 。 为 设计 一 个 可 实现 的 控制 器 ,还 
需 进一步 研究 集 总 不 确定 性 电 " (#,x,), 其 中 6==[&1,62] ,x, 一 [Xx4 ,4 ,区 J]」 。 由 于 gq= 
LI = = yrs | d= ,可 得 
H’* :=M(é€; Twa)Bi(é 2G 4+za) 十 
C(E] 十 元 6 十 有 (6 ;ia) BE ,za) (7; 51) 
RBF-NN 具有 通 近 任何 连续 函数 的 学 习 能 力 。 为 了 设计 一 个 输入 较 少 的 高 效 控制 器 , 假 
设 轨 迹 被 理想 的 跟踪 二 [0,0]" , 则 重 写 瓦 (0,z, ) 一 瑟 Cz,)。 通 过 从 于 (&,zx, ) 中 去 除 &, 输 入 
数量 可 以 减少 到 3n, 这 极 大 地 简化 了 RBF-NN。 考 虑 到 实际 情况 ,匹配 误差 为 6, (6,z,) :一 
H(zx,)—H’* (ez )。 
由 于 瓦 是 由 于 属性 开导 致 的 一 阶 导 数 可 微分 ,因此 将 均值 定理 应 用 于 s, ,可 得 : 
1s Cté,xz |< ole ll (7. 52) 
其 中 ,g : 及 h> 肥 是 一 个 严格 增加 且 全 局 可 道 的 特定 函数 。 
本 节 应 用 简化 的 RBF-NN 近似 互 (zx, ) 为 式 (7.39) ,H(zx,)==® (zx,)0”" 一 6; ,6 是 近似 


误差 ,9" 定义 为 : 
0” =arg min( sup |H(z, — 和 (xz, 10))1) (7 63 
i€EQ, ,EQ 
考虑 李 雅 普 诺 夫 函 数 
VD =ViCD) 十 二 PT (7. 54) 
基于 式 (7. 49) 修 改 后 的 控制 器 如 下 : 
和 
(人 
COS 7 
2&2， 
rz 一 一 : — (Kpi+K,)é, —K,sgn(é,)+@'0 (7.55) 
El 
wor (he) 
2 1 


其 中 ,一 K,sgn(&,) 用 于 以 消除 干扰 rs 和 近似 误差 6, ,同时 增强 系统 的 鲁 棒 性 ,而 B'9 则 用 
于 近似 集 总 不 确定 性 。 与 式 (7. 49) 相 比 ,一 K,é&s 分 为 两 部 分 ,其 中 KK,1 、K,s 是 正定 对 角 和 所 
阵 , 一 K ,1é。 用 于 保证 Vi (i) 的 稳定 性 ,一 K ,s&s 用 于 补偿 ej。 定义 8: 一 6, 十 tq ,A 二 diag(A1， 
ha) 二 diag(Y1 ，Y2 ，…,Y21 )。 神 经 网 络 参数 的 更 新 规则 设计 如 下 : 
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| = Ta Le 
: (7. 56) 
一 一 放风 和 — KjDni =1,2,%" 
其 中 ,i=j 一 1,2，…,7。 
对 式 (7. 54) 求 导 , 并 将 式 (7.55) 和 式 (7.56) 代 入 式 (7. 54) 中 ,可 得 
E 20= > -07 (6; 十 907) 过 < Pt 5 十 0 50 ) (人 


i=1] 
考虑 e， | 委 |e Re 2(&€ 二 [&1 ,8&7), 可 得 不 等 式 人 es, 三 |é,||tlg (#1)=< 
lel?g cel),—é2K,é, <—xr|é | «rll ,其 中 ,x 二 Xn (Ks)。 选 择 xg (181)， 
,S06, +z, pi 


2 ks nél; 方 分 基 关 
Vi() <— Sk; i( EIJK Knés — 70"A0+ 20; TIA0” 扫 一 oV() 十 c 


1 一 1 ci 


(17,58 
1 关 工 关 
其 中 ,c= 二 50 TA ,0 满足 


Sy a RR 


min 


.一 一 一 (7. 59) 
A eM A 


ny 


pe =min(k ,Rk, hes ),R 


将 式 (7. 58) 的 两 边 同时 乘 以 可 以 得 到 所 (eV(1)) 之 ee, 对 其 积分 可 得 VerV0)+ 


ts 考虑 6 ,可 得 本 61 Te <V(0O)+ 方 。 因此 ,假设 V(0) EL 成 立时 , 基 


于 控制 器 式 (7. 55) 和 更 新 规则 式 (7. 56) ,&1 渐 近 收敛 至 紧 集 2, = 1 ||& 1<2zvo+ 关 


3. 平滑 控制 规律 
使 用 上 述 控制 律 式 (7. 55) 可 能 会 引起 抖动 ,可 以 通过 边界 理论 消除 ,用 sat(。) 代 替 
SS 人 (5 


| eu S31 
sat(é,) =4é/6 —1<é,/0<1 (7. 60) 
二 1 é€, 二 一 
其 中 ,2 为 边界 层 厚度 。 
é11 
2 
2 né11 
a (让 ) 
z 王 一 : 一 (Ki +K,2)és —K,sat(é)+8'O (7.61) 
Ei 
2 
Ne ER 
7.3.3 控制 器 仿真 


这 里 使 用 两 连 杆 机 械 辟 验证 所 设计 的 控制 器 的 有 效 性 ,其 动力 学 方程 如 下 : 
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Mi Miz| qi 一 cda cg a)| lai Pig Tl 
十 十 = (7. 62) 
M2 Mz,| |3, co 0 g p2g T2 
Mil 一 (zi 十 maz)204 十 ay22 + 2m,lilscos(gq,) 


Mi, 一 my12 + 2m2l1lscos(g,) 


ee 2 
Mo 二 72272 


其 中 : 


c=m,lil,cos(g,) 
Pi=(mi 二 ms)licos(gs) 二 m2lscos(gi 十 q,) 
pi=m2l2cos(qgi gq2) 
且 =1m,l,=0.8m,mi=0.5kg,m,=0. 5kg,g =9.8。 
参考 轨迹 由 下 式 给 出 : 


Qr1 2. .2935: 二 0, 281n (nt) 
gq, (t) = 一 (7.63) 
gu- 一 1 8961 十 由 Ceoskrty 


假设 g, = 二 g, (0)=[2.2935, 一 1.6961]7 。 机 械 臂 的 初始 位 置 为 ， 
gO [2 (7. 64) 

下 面 将 比较 理想 情况 下 ( 即 H“ 是 完全 已 知 的 ) 的 控制 器 式 (7.48) 和 式 (7. 60) 中 考虑 模型 
不 确定 性 的 实际 控制 器 (即使 用 RBF-NN) 的 性 能 。 将 在 以 下 4 种 情况 下 进行 仿真 ,其 中 情况 
1 和 2 在 自由 空间 中 进行 而 没有 任何 外 力 ( 即 ry=[0,0]7), 情 况 3 和 4 在 扰动 力矩 +, = 
[8sint ,8sint] ”下 进行 。 情 况 1 和 情况 3 使 用 式 (7. 48) 中 的 控制 器 ,情况 2 和 情况 4 使 用 
式 (7. 60) 中 的 控制 器 来 跟踪 式 (7. 63) 的 参考 轨迹 。 

1. 自由 空间 运动 

情况 1 和 情况 2 的 仿真 结果 分 别 如 图 7. 16 和 图 7. 17 所 示 ,机械 臂 在 没有 任何 外 部 干扰 
的 自由 空间 中 ,跟踪 具有 输出 约束 的 参考 轨迹 。 从 图 7.16(a)、(c) 和 图 7.17(a)、(d) 可 以 看 
出 ,两 控制 器 即 式 (7.48) 和 式 (7. 60) 都 能 保证 自由 空间 运动 中 良好 的 轨迹 跟踪 性 能 ,而 控制 器 
即 式 (7. 48) 则 更 好 。 图 7. 16(b) .图 7.17(b) .图 7.18(b) 和 图 7.19(b) 表 明 , 由 于 边界 理论 的 
引入 ,输出 力矩 没有 抖动 。 此 外 ,输出 状态 严格 受 v 限制 。 

对 于 图 7.16(a) .图 7.17(a) .图 7.18(a) 和 图 7.19(a), 由 于 控制 器 的 跟踪 效果 很 好 ,两 条 
曲线 基本 重合 。 

2. 外 部 干扰 下 的 运动 

情况 3 和 情况 4 的 仿真 结果 分 别 如 图 7. 18 和 图 7. 19 所 示 。 与 图 7. 19 相 比 ,控制 器 
式 (7. 60) 在 机 械 臂 收 到 外 部 干扰 时 ,具有 和 鲁 棒 性 。 进 一 步 研究 图 7. 18(b)、(c), 可 以 看 到 ,在 
时 刻 :一 2s 和 +: 二 5s, 控 制 器 的 输出 力矩 突然 变化 ( 见 图 7.18(b)); 同时 ,输出 状态 达到 约束 的 
边界 ( 见 图 7.18(c))。 这 些 仿 真 结果 证 明 式 (7. 48) 中 的 约束 设计 有 效 。 基 于 BLF 设计 的 控制 
器 ,通过 控制 力矩 的 突然 变化 将 输出 状态 拖 回 到 约束 设 定 区 域 。 

接 下 来 比较 图 7. 18(c) 和 图 7. 19(c) ,可 以 清楚 地 发 现 控制 器 式 (7. 60) 的 跟踪 误差 更 小 ， 
输出 力矩 也 更 平滑 。 虽 然 在 自由 空间 中 运动 时 ,控制 器 式 (7. 60) 的 跟踪 精度 不 高 ,但 当 存在 不 
确定 性 和 未 知 干扰 时 , 它 的 跟踪 效果 总 体 上 优 于 控制 器 式 (7. 48) 。 
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(a) 联合 空间 中 的 跟踪 结果 ， 其 中 gn 、qw 代 表 参 考 轨迹 ，g1、g; 代 表 实际 轨迹 
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(b) 施加 在 机 械 辟 上 的 控制 力矩 
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(c) 仿真 过 程 中 的 跟踪 误差 


图 7.16 利用 式 (7.47) 的 控制 器 在 自由 空间 中 进行 轨迹 跟踪 的 仿真 结果 
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(c) 仿真 过 程 中 的 跟踪 误差 


图 7.17 使 用 式 (7. 59) 的 控制 器 在 自由 空间 中 跟踪 轨迹 的 仿真 结果 


图 7.18 
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在 外 部 干扰 下 使 用 式 (7. 47) 的 控制 器 进行 轨迹 跟踪 的 仿真 结果 
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图 7.19 在 外 部 干扰 下 使 用 式 (7.59) 的 控制 器 进行 轨迹 跟踪 的 仿真 结 
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7.4 基于 有 限时 间 收 敛 的 机 械 辟 全 局 自 适应 神经 网 络 控制 


、 本 节 将 介绍 一 类 具有 有 限时 间 (Finite Time,FT) 收 敛 学 习性 能 的 非 线性 机 器 人 的 全 局 自 
适应 神经 网 络 控制 。 

随 着 机 器 人 技术 的 快速 发 展 和 广泛 应 用 ,在 不 同 环境 下 对 精细 的 工作 和 灵活 的 控制 的 需 
求 不 断 增长 。 然 而 ,由 于 机 器 人 动力 学 模型 的 复杂 程度 的 增加 ,特定 的 机 器 人 动力 学 知识 在 实 
际 工程 应 用 中 已 经 不 再 适用 。 男 外 ,工作 条 件 可 能 变 得 更 加 不 确定 ,例如 外 部 环境 的 变化 和 有 
效 载荷 的 变化 等 。 因 此 ,克服 上 述 不 确定 因素 是 机 器 人 控制 的 关键 问题 。 

在 实际 的 工业 应 用 中 ,有 很 多 方法 用 于 克服 系统 中 的 不 确定 性 。 神 经 网 络 (Neural 
-Network,NN) 通 过 提高 控制 系统 在 不 确定 环境 下 的 表现 ,在 提高 机 器 人 智能 程度 方面 发 挥 了 
重要 作用 。 神 经 网 络 可 以 通过 不 同 的 方法 与 控制 相 结 合 , 并 取得 了 显著 的 成 果 。 然 而 ,必须 注 
意 神 经 网 络 的 台 近 能 力 仅 在 紧 集中 有 效 。 常 规 的 基于 神经 网 络 的 机 械 辟 控制 器 只 能 保证 半 全 
局 一 致 有 界 收敛 (Semiglobal Uniform Ultimate boundedness, SGUUB) 的 稳定 性 。 在 这 种 情 
况 下 ,为 机 械 辟 构造 一 个 全 局 神经 网 络 控制 器 是 理想 的 选择 。 值 得 注意 的 是 ,传统 的 自 适应 神 
经 网 络 控制 仅仅 关注 控制 上 的 性 能 ,而 非 神经 网 络 学 习 的 性 能 。 而 常规 的 自 适 应 律 ( 如 梯度 
法 、e 自 适应 律 .8 自 适 应 律 等 ) 无 法 保证 学 习 到 的 神经 权重 能 够 收敛 到 最 佳 值 ; 同时 ,不 能 设 
定 学 习 时 的 收敛 速度 。 事 实 上 ,神经 网 络 的 权重 通常 不 能 收敛 至 最 优 值 。 因 此 ,即便 是 针对 重 
复 的 任务 ,也 需要 在 下 一 次 任务 时 ,重新 进行 神经 网 络 学 习 。 

有 限时 间 (FT) 自 适应 算法 可 以 用 于 提高 神经 网 络 学习 的 性 能 ,如 可 以 由 估计 误差 建立 自 
适应 律 实现 FT 收敛 。 需 要 注意 的 是 ,FT 自 适 应 律 可 以 保证 权重 的 估计 值 在 有 限 的 时 间 里 收 
敛 到 最 优 值 。 因 此 ,通过 神经 网 络 的 学 习 过 程 所 获得 的 最 优 权重 可 以 在 下 一 次 重复 相同 的 控 
制 任务 时 继续 使 用 。 通 过 这 种 方式 ,FT 自 适 应 学 习 算法 减少 了 在 面 对 重 复 控制 任务 时 的 计 
算 负担 ,并 且 提 升 了 瞬 态 性 能 。 因 此 ,FT 自 适 应 学 习 算 法 在 NN 的 实际 应 用 中 发 挥 着 很 重要 
的 作用 。 

本 节 针 对 具有 未 知 动力 学 参数 的 非 线 性 机 械 臂 的 问题 ,将 介绍 一 种 具有 FT 学 习 收 敛 的 
神经 网 络 设 计 全 局 自 适应 神经 控制 器 。 

7.4.1 问题 描述 

1. 系统 描述 

n 连 杆 机 械 臂 动力 学 方程 描述 如 下 : 

M(o)5 二 Co 十 G(q) = (7. 65) 
其 中 ,gq 二 [gi,…,q,] ER" ,一 [as ER" 一 [oa ER" ,分 别 表示 机 械 辟 
关节 位 置 向 量 .关节 速度 向 量 和 关节 加 速度 向 量 。M(q)ER" "Clg,9)ER”"” 和 G(g)€ 


R ”分别 为 惯性 矩阵 、 科 里 奥 利 力 和 向 心力 矩 矩 阵 和 重力 矢量 。 以 下 属性 适用 于 机 械 臂 模型 
式 (7.65)， 和 


性 质 1: 矩阵 M(o)ER” ”是 对 称 正 定 的 。 

性 质 2: 矩阵 M 一 C(q,0)0 是 斜 对 称 的 , 即 z'(M 一 2C)z==0, VzER"”。 

性 质 3: 和 矩阵 Ml(g)、Clg,9) 和 G(g) 都 是 有 界 的 。 

2. 系统 变换 

定义 Xi;=g; ,i=1y*… n= 二 [zi rin] ER",Xy =g;(i=1l, on) X= [ras, 
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zz ER” 。7 连 杆 机 械 臂 是 一 个 多 输入 多 输出 (Multiple-Input Multiple-Output, MIMO) 矢 
量 函 数 系统 ,可 将 其 转化 为 以 下 形式 : 


Z|! 一 工 ? 
= 一 (Cz* 十 G) 十 r (7. 66) 
1 
其 中 ,y= 二 x) 是 机 械 臂 系统 的 输出 方程 。 
7.4.2 控制 器 设计 
1. RBF 神经 网 络 
本 节 作 为 一 个 通用 的 副 近 器 来 模拟 任何 的 实 连续 函数 f: RV 一 R ,函数 形式 如 下 : 
f (Xn) =f (XW” ) He Xn), VX E Qx, (7. 67) 
其 中 F(X;,, 矿 ) 二 WTS(X;,),X;, 属于 0;,CR” 代表 输入 向 量 , 神 经 网 络 的 输出 AER 是 函 
数 太 的 估计 值 , 砂 王 [6 ,…,ON]I 是 权重 参数 向 量 ,m 为 神经 网 络 的 节点 数量 ,W” 是 神经 网 络 
的 最 佳 权重 ,W” 可 定义 为 : 
Ww” =argmin[, sup "Rs Wy (7. 68) 


s(Xi, ) 是 神经 网 络 的 通 近 误差 ,由 以 下 公式 约束 : 
| 二 (7.69) 
S(X;,)==[S1(X;,),…,Snw(X;,)]! 是 输入 的 一 个 非 线性 向 量 函 数 , 具 体 如 下 : 


ee | 
YD 2 


SX ep- 1， 入 


式 中 ,&, 二 [8&1 ,8;s，… ,Sm]' ER” 则 表示 第 ;个 基 函 数 的 中 心 , 是 变量 。 

假设 1: 最 优 权重 W” 在 紧凑 域 0 上 有 限制 | W* | <Ww,,。 

假设 2: 参考 信号 g, 及 其 导数 qv 是 平滑 且 有 界 的 函数 。 

2. 部 分 持续 激励 条 件 

在 自 适应 系统 中 ,持续 激励 (Persistent Excitation,PE) 条 件 具有 重要 意义 ,而 且 当 跟踪 参 
考 周 期 性 轨迹 时 ,RBF 神经 网 络 可 以 在 部 分 PE 条 件 下 实现 特定 周期 轨迹 附近 的 闭环 系统 动 
力学 的 精确 近似 。 

定义 1: 如 果 存 在 二 0,V 二 0, 则 将 向 量 或 矩阵 Y(z) 称 为 持久 激励 , 即 : 

| vi var, Vt 宇 0 (7.70) 


3. 有 用 的 函数 和 关键 的 引 理 
引 理 1: 对 于 任何 wo 之 0 和 w%ER, 以 下 不 等 式 成 立 : 


0<| 7 |— ntanh (2 ) < no rt 
0 


其 中 ,x 是 满足 x 二 e “+? 的 常数 , 即 x 二 0. 2785。 , 
引 理 2: 对 于 连续 系统 = 二 f(x ,1),f(0,1) 二 0,x ER" ,存在 连续 可 微 的 正定 函数 V(z， 
t) 和 实数 a >0,0<ay <1; 当 V(z;,t) 声 一 a1V”(z,t) 成 立时 ,对 于 任何 给 定 的 初始 条 件 


z(to) ,VCz,2) 在 有 限时 间 内 以 建立 时 间 T<( )V ”zcto) to) 收敛 到 零 。 


RTX 二 一 0 
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定义 2; 给 定常 数 0 过 1r ,过 ?rj,; (i 二 1,…,n,j 二 1;2), 作 为 定义 紧 集 W, 边界 的 常数 ， 
切换 函数 集 如 下 : 


1 时 
2 2 2 (i } 
ed Th Ep \g( re te) 
by (zh 9 区 这 3 本 e gy si ug 过 | De Es 时 | ?2) 
Thi Rf 
0 当 | 并 Ai | 时 
区 了 
下 二 二 [ors (7. 73) 


“其 中 ,T= 二 xTER"yz2=[z1i2] ER 以 及 B,(z,)— diag(b;,, (FZ 7 (元 7 面 六 0 
b 宇 1。 

4. 控制 器 设计 

控制 器 的 设计 过 程 分 为 两 个 步骤 。 

第 一 步 : 定义 机 械 臂 关节 位 置 误 差 为 2; 一 zi 一 zid, 其 中 ,zivy 一 av。 取 zi 的 导数 并 使 
用 式 (7. 66) ,可 得 : 


天 | 一 站 | 一 之 7 一 并 一 之 1 (7: 74) 
其 中 Td = , 取 Ty, 作为 式 (7. 74) 的 虚拟 控制 ,并 将 信号 Toaq 设计 为 : 
i ee (775) 


其 中 , 民 iD 一 diag(RN Ri) ki >0,i 王 1, nn; Kis=diag(kiz,1 "kin ) 2 一 0， 
es ye Ns 
2 =wy Bia 
=7xs— Xz rog X14 
=7,— Klzx) 一 天 记 35IC20 QE 76) 
第 二 步 : 将 机 械 臂 关节 速度 误差 表示 为 ,二 xs 一 +24s。 取 xs 的 导数 ,并 结合 式 (7. 66)， 
可 得 : 
Mz, 十 Cz 一 rz 一 Mi —Cray—G (Ny 
式 (7.77) 中 ,向 量 Mz, 是 机 械 臂 关节 加 速度 4 的 函数 , 它 对 测量 噪声 敏感 。 为 了 使 控制 
设计 独立 于 关节 加 速度 ,将 式 (7.77) 改 写 为 如 下 形式 : 
天 (z) 十 F;(z) 一 rc 十 Fi(z) (7.797 
其 中 ,Fi 二 Mz,,F,= 一 Mx, 十 Cz, Fi 一 一 MI 一 Cr 一 G， 
利用 神经 网 络 分 别 允 近 未 知 函 数 下 | .下 , 和 下 , ,可 得 : 
F| 一 ME =W} TS, (z) 十 si | 


F, =—Mi, Cr, =W;'S,(z)++e, Cs 20 
下 ;一 一 Mi 一 Cr —G—=W;!S,(z) ee | 
其 中 ,z= 二 [gq ,9 ,x24 ,24 ] ,WY ,W2 ,Ws3 ”是 最 佳 的 神经 网 络 权 重 和 矩阵 。S1,S, ,Ss 是 基 范 
数 向 量 ; el ,es ,e; 为 神经 网 络 的 构造 误差 ,其 中 el | <sy ,sy | <s*，| es | <e: 。 
式 (7.79) 可 以 进一步 表示 为 : 
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Fs 一 SI1W1， 4 
了 上 2,: =S2W2, Re (7. 80) 
Fae =S3W;., bg 
其 中 ,i 二 1,…,n。Wi7; Wi; 分 别 为 矩阵 Wi 、W2， .Ws 的 第 i 列 。 
使 用 RBF-NN 方法 , 式 (7.78) 可 以 分 为 ”个 子 系统 : 


SIW; ,十 上 ;十 STW7 十 sy ;一 SI 一 el ;一 ri (7. 81) 
定义 新 的 权重 和 矩阵 如 下 : 
Wi 
WW = We (7..82) 
六 
因此 , 式 (7. 81) 可 以 表示 为 : 
S™W 十 Ei 一 ri (7. 83) 


RP 0 0 , 
Ng 一 六 十 N 十 NEi 一 El Ts = 


设计 如 下 自 适应 控制 器 : 
SR 一 EE 
(1—6b,,;(z,))ur, i 二 ly CR: 84) 
uN =WIS,(z) (7. 85) 


A 


ur =FY,(z)tanh (7. 86) 


已 2 
其 中 ,天 2 =diag(kz1,1°" ,R21,n) R21,i>0,i=1, 2, 开 22 一 diag(R22,1，…22n)，R22.i 一 0， 
i 二 1,…,n,F9y,(z) 是 下 ,,;(z) 的 上 界 。 
为 了 便于 权重 估算 ,设计 如 下 过 滤器 : 


p51 +S1 =S1,S1 | =。 一 ONs 


(7.87) 


Di ri tr = 0 
其 中 ,Sijy ,Ss ,Sar 和 rir 分 别 是 Si,S,,S， 和 zi 的 滤波 形式 。 根 据 式 (7. 83) 和 式 (7. 87)， 
可 得 : 
S; 一 S . 
Ge (7. 88) 
Ns XN: Ns 
已 知 矩阵 PER “” “,Q,ER“ ,可 以 定义 成 以 下 形式 : 
P=—tP+S,S? | 


WwW:*T 


; 和 (7. 89) 
Q;=—tQ;+S ri 
其 中 2>0 为 设计 参数 。 式 (7. 89) 的 解 如 下 : 
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PC) -|'e Sr | 
(7. 90) 
Qi (1) -| | 
定义 辅助 向 量 E, ER” , 它 根据 P、Q,; 计算 : 
E,=PW., —0,=PW— PW —y,=—PW,—y, (7. 91) 
其 中 ,0; =PW; +y; ,并 有 yi -| edr. 
闭环 误差 方程 变 为 : 
M3, 十 CE 一 一 — Kyoz, — Kysign(z,) + Bs(z,)(F, tes)+ 
(I—B,(z2)) (Fs — ur) (7. 92) 
其 中 I = (z)=WI!S, (z) ,Ws =W;,, 一 见 ，， ,W, =W; —W, o 
估计 参数 的 自 适应 规律 设计 如 下 : 
人 ee ws PIE Ss 
WwW, =T (5,z2.b2. 22) —6 :TE 一 一 | (i=1,",n) (7. 93) 
其 中 ,TT 是 对 称 正定 矩阵 ,6; 是 设计 正 参数 。 
5. 稳定 性 分 析 
考虑 系统 式 (7. 65) ,控制 律 式 (7. 84) 和 式 (7.93) 。 选 择 李 雅 普 诺 夫 函数 如 下 : 
=V 十 V， (7. 94) 
其 中 
Vi 一 也， 人 7957 
V, = XI Mz + 十 二 1 SET i (7. 96) 
i=1 
式 (7. 96) 中 有 是 一 个 正定 矩阵 。 
对 Vi 求 导 ,可 得 : 
Vi =Z1 廊 | 
=z1 (7z, — Kz — Ki,sign(z))) 
一 -1 及， 12Z1 一 PN 
1 一 1 
<— > (Ri ;| 无; ;| ) +z1z, 
i=1 
<— KY VV +z1z, (7. 97) 


其 中 ,KT 3 DA CK) 
根据 式 (7. 91) ,可 得 : 
a(P 1E.,) a(W; +P 1y,) 
= 
=—W,+Pp Pp iy,— Py, 
= 一 W, 十 多 
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入 


=W; ty (7. 98) 
其 中 , y=P 1PP 1y, 一 P 1y,。 
同样 对 V, 求 导 ,可 得 : 


六 = 二 TME 十 了 NI， 十 >) ETPTIT ! W, +y)) 
i 一 1 


一 了 NI 让 二 元 “Ro Ko = (R22 2 | 号 | 于 全 BD2( eTF 
zi(I—B,(z,))(F; — ur)+ 91ETp7ir7 座 ,) 一 Py oy (7. 99) 
将 自 适应 法 则 式 (7.93) 代 入 式 (7.99) ,有 ， 加 
V, 一 一 3 元; 一 关 Kx, 一 S (&22 ，| 序 。 ;| ) il B, (zo)es ti2 (IT — B, (zs)) (Fs 一 zxR) 十 
这 1 
(一 籽 P1S; 元 ;了 ;( 却 ? )) 十 y (Erp 太一 人， i (7. 100) 
以 下 不 等 式 成 立 ， I 
i 去 Ses lz |) 
i=1 
[Ps 二 EN Wi 
使 用 Young 不 等 式 , 可 得 : 


V, 委 一 2 2) 一 二 2 下 丰 殊 二 Fy 2 i 2 >) (cos | 这 i219 一 
1 2 4 
2 lz | Myrp SD— 2 EFIG, — IP r yd)) 
Es=1 fA 


<— Cy | (R22,; eg 一 kw 十 | 并 8 | 


i=] 
>) ETNCG; 一 1P 0 1)) 一 去 了 
i 一 1 


SS | (7.102) - 


式 中 ,K2 = 二 min [(R2 ;一 sy ;一 kws 十 giP 1S, | )X i r= BE Ewes 
"es 
V=V +V, <—K* VV (7. 103) 
其 中 ,K* = 二 min[KY ,K: ]。 
注意 , 式 (7. 103) 满 足 7.4. 3 节 中 引 理 2 的 条 件 。 因 此 ,可 以 保证 跟踪 误差 z， 、z。 和 参数 
误差 W 的 有 限时 间 收 剑 。 然 后 ,V=0, Yt 这 1, ,其 中 有 限时 间 上 有 : 
a < RK MYC6) (7. 104) 
跟踪 误差 二 和 x, 在 有 限时 间 内 消失 为 零 ,因此 跟踪 误差 去 、z。 可 以 保证 为 零 。 
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需要 注意 的 是 ,由 式 (7. 97) 和 式 (7. 98), 耗 散 性 和 7, 一 1 方法 可 以 结合 到 李 雅 普 诺 夫 矣 
数 中 ,以 确保 鲁 棒 性 。 


7.4.3 仿真 


对 两 自由 度 机 械 辟 模型 进行 仿真 ,如 图 7. 20 所 示 ,分 析 上 述 控制 器 的 有 效 性 。 
两 自由 度 机 械 辟 的 动力 学 描述 如 下 ， 


| 证 全 | ne | 
q q Ra se 
Mal M,, Vz V2» G, 


(7. 105) 
其 中 ,m1 二 2kg,m, 王 0.85kg,l1 二 0.35m,l, 二 0.31m,， 
1 二 0.061kgm’ ,1 二 0.020kgm? 。m; 是 连 杆 i 的 惯 
性 ,I; 是 连 杆 i 在 连 杆 i 的 质量 中 心 处 绕 轴线 的 惯 
性 。2; 和 !. 是 连 杆 i 的 长 度 和 第 i 一 1 个 关节 到 第 ; 7 
个 关节 重心 的 距离 ,i 分 别 为 1.2。 两 自由 度 机 械 臂 7.20 两 自由 度 机 械 臂 结构 
参数 如 表 7. 1 所 示 。 


表 7.1 两 自由 度 机 械 臂 参数 描述 


Mi milzi 十 mz (1? 十 L121 十 2l11462cosqs) 十 Ti 十 I, 

Miz m2 (Ll?, tlilzcosqs)+I1, 

M，， mz (KZ2 十 1 1.acosgy) 十 T2 

M,, m2lez tI 

Vu —m2lil.s 42Sing? 

Vi —mo2lilcs (qd1+q2)sing, 

Val m2l1l.191sing; 

V2 0 

GI (milist+moli)gcosgqi m2l.sg cosCgq1gq,) 
G, m2l.2sg cos(qgi+gq,) 


期 望 轨迹 为 go 二 3sin0. 5t 和 qs 二 2cos0. 5t ,其 中 t+€E[0,tyj],tjy 二 15s。 控 制 增益 选择 为 
Ki 一 diag(10,10), Ki, = diag(0. 0001,0. 0001), K, = diag (10,10), K,,= diag(0. 0001， 
0.0001)。 同 时 ,神经 网 络 自 适应 律 设 为 Ts = 二 15I1 ,Tp = 二 151,65 二 0.005,6p 一 0.005。 神 经 
网 络 的 隐藏 层 节点 数 为 N= 二 N, 二 NN, 二 256。 神 经 网 络 权 重 矩 阵 初始 化 为 WW (0)= 二 0 ER ， 
多 (0) 一 0ER"% ,参数 设置 为 w2 二 0. 01。 且 有 :ri 和 一 
2 “7 三 下 及 及 和 二 于 和 0 三 1。 仿真 中 ,o 修改 自 适应 定律 如 下 : 

W, =T (Ssz,.,b2.; (zs) —o,W,) (7. 106) 

仿真 结果 如 图 7. 21 一 图 7. 28 所 示 。 

图 7.21 一 图 7.24 表明 ,两 种 自 适应 控制 器 都 可 以 在 有 限时 间 内 跟踪 参考 信号 vv 和 dv， 
且 控 制 性 能 稳定 。 然 而 ,与 传统 的 自 适应 律 相 比 ,具有 FT 学 习 自 适应 律 的 全 局 自 适 应 神经 控 
制 器 ( 见 式 (7. 94)) 跟 踪 误差 收敛 速度 更 快 ,这 是 由 于 采用 了 自 适 应 法 则 的 导出 权重 误差 信息 
E,。 因 此 ,用 于 机 器 人 末端 执行 器 的 全 局 自 适应 神经 控制 器 能 够 在 未 知 动 态 下 对 跟踪 误差 实 
现 良 好 的 瞬 态 跟踪 性 能 。 
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一 0- 修改 适应 
3 一 一 有 限时 间 适 应 
2 “< 参考 
1 
Eo 
Sn 
| 
> 
本 
-40 2 4 6 8 10 12 
时 间 /sec 
图 7.21 gi 的 跟踪 性 能 
2 一 o- 修 改 适应 
LS 一 有 限时 间 适 应 
1.0 -= 参考 
0.5 
BE 0 
六 
-0.5 
-1.0 
本 了 
2 2 4 6 8 10 12 
时 间 /sec 
图 7.22 gq, 的 跟踪 性 能 
2 Y 
~ 一 C- 修 改 适 应 
一 有 限时 间 适 应 
--- 参考 
4 
县 -6 
8g 
-10 
= 
0 2 4 6 8 10 12 
时 间 /sec 
图 7.23 di 的 跟踪 性 能 
25 
20 一 o- 修 改 适应 
一 一 有 限时 间 适 应 
15 = -= 参考 


Gd2/rad/s 


6 
时 间 /sec 
图 7.24 g, 的 跟踪 性 能 
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时 间 /sec 
图 7.25 第 一 个 关节 的 FT 权重 更 新 


6 
时 间 /sec 
图 7.26 第 二 个 关节 的 FT 权重 更 新 


时 间 /sec 
图 7.27 第 一 关节 的 常规 权重 更 新 
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0.015 


0.010 


时 局 有 
图 7.28 第 二 关节 的 常规 权重 更 新 
图 7. 25 和 图 7. 26 说 明 权重 能 在 有 限时 间 内 收敛 到 最 佳 值 。 与 图 7. 27 和 图 7. 28 的 结果 


相 比 ,FT 自 适 应 学 习 算法 可 以 保证 权重 的 估计 值 在 规定 时 间 范 围 内 收敛 到 最 优 值 , 使 得 学 习 
权重 可 以 用 于 重复 任务 ,从 而 减少 计算 负荷 ,提高 系统 的 瞬 态 性 能 和 和 鲁 棒 性 。 


7.5 本 章 小 结 


本 章 首先 介绍 了 神经 网 络 控制 的 基本 原理 ,然后 结合 几 个 具体 的 机 器 人 控制 研究 课题 讨 
论 神经 网 络 控制 器 的 设计 ,每 个 控制 器 都 针对 不 同 的 应 用 场景 ,读者 可 以 根据 自己 的 研究 方向 
和 兴趣 进行 深入 人 研究。 


CHAPTER 8 


自 适 应 控制 的 研究 对 象 是 具有 一 定 程度 不 确定 性 的 系统 。“ 不 确定 性 ?是 指 描述 被 控 对 象 
及 其 环境 的 数学 模型 不 是 完全 确定 的 ,其 中 包含 一 些 未 知 因素 和 随机 因素 。 任 何 一 个 实际 系 
统 都 具有 不 同 程度 的 不 确定 性 ,这 些 不 确定 性 可 以 表现 在 系统 内 部 ,也 可 以 表现 在 系统 外 部 。 
从 系统 内 部 来 讲 , 描 述 被 控 对 象 的 数学 模型 的 结构 和 参数 ,设计 者 事先 并 不 一 定 能 准确 知道 。 
作为 外 部 环境 对 系统 的 影响 ,可 以 等 效 地 用 多 个 扰动 来 表示 。 这 些 扰动 通常 是 不 可 预测 的 。 
此 外 ,还 有 一 些 因 测 量 引 起 的 不 确定 因素 进入 系统 。 面 对 这 些 客观 存在 的 多 种 不 确定 性 ,如 何 
设计 适当 的 控制 系统 ,使 其 某 个 性 能 指标 达到 并 保持 最 优 或 者 近似 最 优 , 这 就 是 自 适应 控制 所 
要 研究 和 解决 的 问题 。 

和 常规 的 反馈 控制 类 似 , 自 适应 控制 也 是 一 种 基于 数学 模型 的 控制 方法 ,但 自 适 应 控制 所 
依据 的 关于 模型 和 扰动 的 先 验 知识 比较 少 , 需 要 在 系统 的 运行 过 程 中 不 断 提取 有 关 模 型 的 信 
息 ,使 模型 信息 逐步 完善 。 具 体 地 说 ,可 以 依据 被 控 对 象 的 输入 输出 数据 ,不 断 地 辨识 模型 参 
数 , 这 个 过 程 称 为 系统 的 在 线 辨 识 。 随 着 生产 过 程 的 不 断 进行 ,通过 在 线 辨 识 , 模 型 会 越 来 越 
准确 , 越 来 越 接 近 于 实际 情况 。 由 于 模型 参数 在 不 断 地 优化 ,那么 基于 这 种 模型 辨识 出 来 的 控 
制作 用 也 将 随 之 不 断 地 改进 。 正 因 如 此 ,控制 系统 具有 一 定 的 适应 能 力 。 例 如 某 些 控制 对 象 ， 
其 特性 可 能 在 运行 过 程 中 要 发 生 较 大 的 变化 ,但 通过 在 线 辨识 和 改变 控制 器 参数 ,使 控制 系统 
逐渐 适应 ,最 终 将 系统 调整 到 一 个 较 好 的 工作 状态 。 


8.1 自 适 应 控制 基本 原理 


自 适应 控制 器 ,能 够 通过 修正 自身 的 特性 以 适应 对 象 和 扰动 的 动态 特性 变化 ,从 而 使 整个 
控制 系统 获得 满意 的 性 能 。 自 适应 控制 器 具有 以 下 特点 : 

。 研究 对 象 具有 不 确定 性 或 难以 确 知 ; 

。 能 消除 系统 结构 扰动 引起 的 系统 误差 ; 

Es 

。 自 适应 控制 是 较为 复杂 的 反馈 控制 。 


B13 ee 
1. 自 适应 控制 基本 原理 
自 适应 控制 的 过 程 如 图 8. 1 所 示 。 


在 线 辨 识 /性 能 计算 控制 决策 


图 8.1 自 适应 控制 的 过 程 
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具体 的 控制 过 程 如 下 。 

@ 信息 采集 模块 获取 被 控 系 统 (或 可 调 系统 ) 的 输入 输出 及 其 相关 状态 信息 等 。 

@) 在 线 辨 识 或 性 能 计算 模块 对 系统 相关 参数 或 状态 进行 连续 或 周期 性 估计 (辨识 ) ,或 者 
对 系统 的 性 能 指标 进行 计算 。 

@ 控制 决策 根据 辨识 结果 与 指标 要 求 , 确 定 当前 控制 (调整 ) 策 略 ,通过 与 给 定性 能 指标 
进行 比较 ,确定 相应 的 控制 决策 。 

@ 通过 修正 装置 对 被 控 系 统 的 控制 器 (或 可 调 系统 ) 的 相关 参数 或 状态 进行 修正 或 调节 。 

关于 在 线 识别 与 性 能 计算 模块 : (1) 并 非 所 有 的 自 适 应 控制 系统 都 要 求 直 接 辨识 被 控 对 
象 的 特性 或 参数 ,对 某 些 自 适应 控制 系统 ,只 要 测量 并 计算 出 性 能 指标 的 数值 ,就 可 以 确定 控 
制 策略 应 如 何 修正 ; (2) 通 常 , 自 适应 控制 系统 需要 选择 一 个 合适 的 系统 性 能 指标 ,并 将 该 性 
能 指标 的 优 劣 与 修正 机 构 对 应 的 修正 信号 联系 起 来 ,而 与 性 能 指标 极 值 点 对 应 的 修正 信号 变 
化 规律 即 为 自 适应 规律 。 

2. 自 适应 控制 系统 的 基本 原理 

自 适应 控制 系统 需要 不 断 地 测量 本 身 的 状态 、 性 能 、 参 数 ,对 系统 当前 数据 和 期 望 数 据 进 
行 比较 ,再 做 出 改变 控制 器 结构 ,参数 或 控制 方法 等 的 最 优 决策 ,从 而 在 保证 系统 输出 满足 要 
求 的 同时 ,保证 系统 的 稳定 。 由 于 自 适 应 控制 系统 形式 各 异 ,差别 较 大 ,通常 不 具有 一 般 性 模 
式 , 因 此 这 部 分 只 针对 其 原理 性 结构 进行 介绍 ,如 图 8. 2 所 示 为 自 适应 控制 系统 的 基本 原理 。 
可 以 看 出 , 自 适应 控制 系统 有 两 个 回路 : 一 个 是 带 有 过 程 和 可 调控 制 器 的 反馈 回路 ; 另 一 个 
则 是 具有 自 适应 机 构 的 自 适 应 回路 。 其 中 , 自 适应 回路 用 来 控制 参数 的 调节 ,该 回路 的 参数 变 
化 比 反馈 回路 的 信号 变化 慢 很 多 ,而 自 适 应 回路 的 控制 参数 调整 速度 则 远 小 于 反馈 回路 的 过 


程 参数 变化 速度 。 
自 适应 机 构 
(参数 调节 ) 


控制 器 参数 


可 调控 制 器 被 控 对 象 
控制 量 


图 8.2 自 适 应 控制 系统 结构 原理 框图 


一 般 的 反馈 控制 系统 对 于 系统 内 部 特性 的 变化 和 外 部 扰动 的 影响 都 具有 一 定 的 抑制 能 
力 ,但 是 由 于 控制 器 参数 是 固定 的 ,所 以 当 系统 内 部 特性 变化 或 者 外 部 扰动 的 变化 较 大 时 , 系 
统 的 性 能 常常 会 大 幅度 下 降 ,甚至 不 稳定 。 也 就 是 说 ,对 那些 对 象 特性 或 扰动 特性 变化 范围 很 
大 ,同时 又 要 求 经 常 保持 高 性 能 指标 的 一 类 系统 ,采取 自 适 应 控制 是 合适 的 。 首 先 自 适应 机 构 
可 以 通过 输入 口 检测 到 变化 ,并 能 够 根据 变化 量 来 修正 控制 器 的 参数 ,然后 将 这 些 参数 置 人 控制 
器 ,从 而 使 控制 器 的 输出 发 生变 化 来 减 小 系统 误差 ,最 终 实现 维持 控制 性 能 最 优 或 次 优 的 目的 。 

正 是 由 于 图 中 的 自 适应 回路 ,使 得 自 适 应 控制 具有 很 强 的 鲁 棒 性 。 同 时 也 应 当 注 意 , 自 适 
应 控制 比 常规 反馈 控制 要 复杂 得 多 ,成 本 也 高 得 多 ,因此 只 是 在 用 常规 反馈 达 不 到 所 期 望 的 性 
能 时 , 才 会 考虑 采用 。 


8.1.2 自 适 应 控制 系统 的 分 类 
按照 不 同 的 准则 , 自 适应 控制 系统 的 分 类 也 各 不 相同 。 若 按 控制 器 参数 获得 的 方法 ,可 将 
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其 分 为 直接 自 适应 控制 和 间接 自 适应 控制 ; 按照 被 控 对 象 的 性 质 , 可 分 为 确定 性 自 适 应 控制 
和 随机 自 适应 控制 ; 按 结构 则 可 分 为 前 馈 自 适应 控制 和 反馈 自 适应 控制 。 

通常 ,目前 被 多 数 人 接受 的 分 类 是 将 自 适应 控制 分 为 自 校正 控制 .模型 参考 自 适应 控制 以 
及 其 他 自 适应 控制 ,前 两 种 自 适应 控制 的 理论 体系 和 方法 相对 较为 成 熟 。 下 面 针 对 这 种 分 类 
介绍 几 种 常用 的 自 适应 控制 。 

1. 前 馈 自 适应 控制 系统 

前 馈 自 适 应 控制 系统 借助 于 过 程 扰动 信号 的 测量 ,通过 自 适 应 机 构 来 改变 控制 器 的 状态 ， 
从 而 达到 改变 系统 特性 的 目的 。 它 与 前 馈 -反馈 复合 控制 系统 的 结构 比较 类 似 , 但 前 馈 自 适 应 
控制 增加 了 自 适应 机 构 ,并且 控制 器 可 调 。 当 扰动 不 可 测 时 ,前 馈 自 适应 控制 系统 的 应 用 就 会 
_ 受到 严重 的 限制 。 该 控制 系统 的 结构 图 如 图 8. 3 所 示 。 


z(h) 
自 适 应 机 构 
可 调控 制 器 


CC 


图 8.3 前 馈 自 适应 控制 系统 结构 


2. 反馈 自 适应 控制 系统 

反馈 自 适应 控制 系统 是 根据 系统 内 部 可 测 信息 的 变化 ,来 改变 控制 器 的 结构 或 参数 ,以 达 
到 提高 控制 质量 的 目的 。 反 馈 自 适应 控制 系统 的 结构 如 图 8.4 所 示 。 从 图 中 可 以 看 出 , 除 原 
有 反馈 回路 之 外 ,反馈 自 适 应 控制 系统 中 新 增加 的 自 适 应 机 构 形 成 了 另 一 个 反馈 回路 。 


车 oe, 
被 控 对 象 


图 8.4 反馈 自 适 应 控制 系统 的 结构 


3. 自 校正 控制 系统 

自 校 正 控制 系统 又 称 自 优 化 控制 或 模型 辨识 自 适应 控制 。 自 校正 控制 有 两 种 形式 : 间接 
自 校正 控制 和 直接 自 校正 控制 。 间 接 自 校正 控制 系统 的 结构 如 图 8. 5 所 示 , 可 以 看 到 ,该 控制 
系统 是 由 被 控 过 程 、 过 程 模 型 参数 估计 器 (在 线 辩 识 ) .控制 器 参数 计算 器 和 可 调控 制 器 组 成 。 

过 程 模 型 参数 估计 器 通过 采集 的 过 程 输入 、 输 出 信息 ,实现 过 程 模型 的 在 线 辨 识 和 参数 估 
计 。 在 获得 的 过 程 模 型 或 估计 参数 的 基础 上 ,按照 某 种 策略 设计 控制 器 ,并 按 一 定 的 性 能 优化 
准则 ,计算 控制 参数 ,使 得 闭环 系统 能 够 达到 最 优 的 控制 性 能 。 

模型 参数 的 估计 方法 有 很 多 ,如 最 小 二 乘法 、 随 机 逼近 法 、 极 大 似 然 法 等 。 控 制 策 略 有 极 
点 配置 .PID、 最 小 方差 控制 等 。 将 不 同 的 辨识 方法 和 不 同 的 控制 策略 搭配 ,又 可 以 组 成 不 同 
形式 的 自 校正 控制 系统 。 
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过 程 参数 


过 程 模型 参数 
估计 器 (在 线 辨识 ) 


图 8.5 间接 自 校正 控制 系统 的 结构 


与 间接 自 校正 控制 系统 相 比 ,直接 自 校 正 控制 系统 省 略 了 控制 器 参数 计算 器 ,并 将 模型 参 
数 估计 器 替换 为 控制 器 参数 估计 器 ,直接 自 校 正 控 制 系统 的 结构 如 图 8. 6 所 示 , 可 以 看 到 , 控 
制 器 参数 估计 器 的 参数 估计 结构 直接 送 到 可 调控 制 器 进行 控制 量 的 计算 。 由 于 直接 自 校正 控 
制 系统 省 略 了 控制 器 参数 估计 器 ,减少 了 大 量 计算 ,提高 了 速度 。 


控制 器 
参数 估计 器 


输入 
CO 可 调控 制 器 被 控 过 程 
E 控制 量 


图 8.6 直接 自 校正 控制 系统 的 结构 


4. 模型 参考 自 适 应 控制 系统 

模型 参考 自 适应 控制 系统 有 多 种 结构 ,并 联 式 模型 参考 自 适应 控制 系统 的 结构 如 图 8. 7 
所 示 。 模 型 参考 自 适应 控制 系统 能 够 在 参考 模型 始终 具有 期 望 的 闭环 性 能 的 前 提 下 ,使 系统 
在 运行 过 程 中 ,力求 保持 被 控 过 程 的 响应 特性 与 参考 模型 的 动态 性 能 一 致 。 该 控制 系统 主要 
由 参考 模型 .可 调 机 构 ,被 控 对 象 科 自 适应 机 构 组 成 。 其 中 ,参考 模型 表达 了 期 望 的 闭环 性 能 ， 
自 适应 机 构 根 据 系统 广义 误差 e(t), 按照 一 定 的 规律 改变 可 调 系 统 的 结构 或 参数 。 广 义 误差 
向 量 e 二 x, 一 x (其 中 xz,, .zx 分 别 表示 参考 模型 的 状态 和 输出 ) 不 为 0 时 , 自 适 应 机 构 按照 一 . 
定 规律 改变 可 调 机 构 的 结构 或 参数 ( 称 为 参数 自 适应 方案 ) 或 直接 改变 被 控 对 象 的 输入 信号 
( 称 为 信号 综合 自 适应 方案 ) ,以 使 系统 的 性 能 指标 达到 或 接近 希望 的 性 能 指标 。 

并 联 模型 参考 自 适应 系统 的 数学 模型 可 以 用 状态 方程 和 输入 -输出 方程 进行 描述 。 对 应 
模型 参考 自 适 应 控制 系统 ,在 描述 其 数学 模型 时 ,有 连续 型 和 离散 型 两 种 系统 。 下面 基于 参数 
自 适应 方案 分 别 对 这 两 种 系统 用 状态 方程 进行 描述 。 

对 于 连续 的 模型 参考 自 适应 系统 ,其 参考 模型 为 : 

和 三 AB WU YT.(0)=26 (8.1) 
在 可 调 参 数 模型 参考 自 适应 系统 中 ,可 调 系 统 为 : 
二 A(e,t)zr+TBle,i)u 


(8.2) 
z(0) 一 zu，A4(0) 一 4A,，B(0) 一 B， 
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自 适应 机 构 


图 8.7 并 联 式 模型 参考 自 适应 控制 系统 的 结构 


其 中 ,w 为 m 维 分 段 连续 的 输入 回 量 ,z， 和 ~ 均 为 n 维 状 态 向 量 ,A,, 和 B, 分 别 为 n 维和 mm 
维 常 数 和 矩阵 ,而 4 和 B 则 分 别 为 n 维和 m 维 时 变 和 矩阵 。 需 要 注意 的 是 , 式 (8. 1) 的 参考 模型 是 
稳定 的 , 且 是 完全 可 控 和 完全 可 观测 的 。 

对 于 离散 的 模型 参考 自 适应 系统 ,其 参考 模型 为 : 


Zn(R 十 1) 一 AZnCR) 十 如 CR)， rm(0)=rno (8. 3) 
可 调 系统 的 参数 自 适 应 方案 的 系统 模型 为 ; 
ZX(k 二 1)=A(e,k)r(k)+B(e,k)u(k) (8. 4) 


其 中 ,x(0)= 二 zo,;4(0)=A,,B(0)=B,。 


8.2 基于 神经 自 适应 观测 器 的 柔性 关节 机 器 人 控制 


柔性 关节 机 械 臂 的 模型 具有 高 度 非 线 性 、 强 看 合 性 和 时 变性 等 特点 ,其 控制 器 的 设计 具有 
很 大 的 挑战 。 同 时 , 它 的 运动 学 和 动力 学 模型 不 可 避免 地 存在 不 确定 性 ,所 以 不 可 能 用 一 个 精 
确 的 模型 来 设计 其 控制 器 。 另 外 ,设计 控制 器 所 需 的 状态 参数 在 实际 工 况 下 也 可 能 是 不 可 测 
的 。 为 解决 上 述 问题 ,本 节 将 介绍 一 种 自 适 应 神经 网 络 观测 器 ,对 模型 未 知 的 单 连 杆 柔 性 关节 
机 械 臂 的 状态 变量 进行 估计 。 

可 编程 机 絮 人 在 自动 化 制造 中 扮演 着 越 来 越 重 要 的 角色 ,尤其 是 在 通过 批量 生产 来 降低 
劳动 力 成 本 的 装配 线 上 。 为 了 满足 每 个 客户 对 产品 的 独特 需求 ,将 会 涌现 大 量 定制 化 产品 。 
众所周知 ,快速 增加 的 产品 类 型 以 及 产品 生产 过 程 的 变化 给 装配 线 的 设计 带 来 了 极 大 的 不 确 
定性 。 重 新 配置 生产 线 需要 对 机 器 人 重新 编程 ,这 将 会 增加 生产 线 的 成 本 。 因 此 ,现代 制造 业 
需要 一 种 灵活 的 方法 来 管理 生产 过 程 的 多 变性 。 制 造 过 程 中 最 灵活 的 因素 是 操作 工人 ,他 们 
对 不 可 预测 的 事件 具有 超越 机 器 的 敏感 性 和 应 对 能 力 ,能 够 对 不 同 信息 进行 快速 处 理 , 在 切换 
任务 时 也 能 快速 适应 。 然 而 ,人 工装 配 虽然 可 以 降低 初始 投资 ,但 由 于 其 自动 化 程度 较 低 ,后 
期 的 人 工 成 本 会 显著 增加 。 综 合 考虑 ,最 好 的 解决 方案 是 利用 人 和 机 器 人 的 协作 来 缩小 完全 
自动 化 生产 线 和 全 手动 装配 之 间 的 差距 。 人 和 机 器 人 的 协作 , 相 较 于 机 器 人 单独 工作 或 人 单 
独 工作 ,将 会 更 有 效率 。 预 计 协 作 机 器 人 最 终 将 比 目 前 在 制造 业 中 使 用 的 非 协 作 机 器 人 的 制 
造成 本 更 低 ,用途 更 加 广泛 。 同 时 ,协作 机 器 人 需要 在 动态 变化 和 非 结构 化 的 工作 环境 中 安全 
地 执行 物理 交互 。 一 种 常见 的 方法 是 利用 柔性 关节 来 提高 机 械 臂 机 械 结构 的 柔顺 性 。 柔 性 关 
节 为 机 械 臂 提供 了 和 柔顺 性 , 当 柔 性 关节 机 械 臂 在 操作 过 程 中 遇 到 障碍 时 ,机 械 臂 与 障碍 物 之 间 
的 接触 力 将 变 得 相对 较 小 。 因 此 ,柔性 关节 机 械 臂 在 人 与 机 器 人 的 物理 协作 领域 得 到 了 广泛 
的 应 用 。 


112 去 下 机 器 人 控制 一 一 运动 学 、 控 制 器 设计 、 人 机 交互 与 应 用 实例 


径 向 基 函 数 (Radial Basis Function,RBF) 神 经 网 络 可 以 逼近 任何 非 线 性 函数 ,可 以 代替 未 
知 的 非 线 性 系统 设计 控制 器 。RBF 网 络 结构 如 图 8. 8 所 示 。 

输入 信号 通过 输入 层 传递 到 隐藏 层 , 隐 含 层 中 的 节点 称 
作 基 函数 ,输出 层 称 为 线性 函数 。 基 郴 数 有 很 多 种 ,本 节 使 
用 高 斯 函数 作为 基 函 数 ,如 下 


(8.5) 


|z=z,; - 
D2 

其 中 ,一 1,2,3，…9qg， zz 一 [zlyzi 和 wz] 是 输入 样本 。 
C, 是 隐藏 层 节点 的 中 心 ,5， 是 高 斯 函数 的 宽度 ,*， 是 隐藏 层 
的 输出 ,9 是 隐藏 层 中 的 节点 数 。RBF 网 络 的 输出 是 隐藏 层 图 8.8 RBF 网 络 结构 


SF C22 一 EXP - 


节点 的 线性 释 加 : 
yd (x) (8. 6) 
j=1 
其 中 ,W; 是 神经 网 络 的 权 向 量 。 通 过 选择 合适 的 权 向 量 ,RBF 网 络 可 以 允 近 任意 精度 的 连续 
函数 : 
“h(Z) =W'S(Z)+e C8 7 


其 中 ,V ZE Q. ,W 是 最 优 权 向 量 ,e 是 逼近 误差 。 

后 文 将 介绍 一 种 基于 RBF 神经 网 络 的 未 知 系统 柔性 关节 机 器 人 观测 器 设计 。 在 此 基础 
上 ,针对 模型 未 知 的 单 连 杆 柔性 关节 机 械 臂 , 设 计 了 基于 动态 曲面 方法 的 控制 器 ,并 利用 RBF 
神经 网 络 建立 机 械 臂 的 未 知 模型 。 最 后 ,通过 仿真 实验 来 测试 和 验证 所 设计 控制 器 的 有 效 性 。 

8.2.1 自 适 应 观测 器 设计 

1. 自 适 应 观测 器 设计 

以 下 介绍 一 种 基于 RBF 神经 网 络 的 状态 观测 器 的 具体 设计 过 程 。 

非 线性 系统 的 一 般 模型 为 : 


Z(t)=f(zr,u) 
(8. 8) 


y(t)=Cx(z) 


其 中 ,uw ER 为 输入 ,y ER 为 输出 ,x ER 为 系统 的 状态 向 量 ,f 为 未 知 函数 ,C 为 系统 
的 输出 矩阵 。 定 义 g(xz,u)= 二 f(z,u) 一 Ax ,可 得 : 


(1) 二 Axz 十 g(x,u) 
(8. 9)- 
y(t)=Czr(t) 
其 中 ,A 为 Hurwitz 矩阵 ,(C,4) 是 可 观测 的 。 
构建 观测 器 模型 如 下 : 
(2) =AL AF,u) Gly 一 C2) 
(8; 10) 
$=CE() 


其 中 ,全 为 观测 器 的 状态 。 选 择 观 测 器 增益 GER”” ,使 得 A 一 GC 是 一 个 Hurwitz 矩阵 。 
采用 RBF 神经 网 络 副 近 非 线性 系统 ,g (x ,u) 可 以 重新 计算 如 下 : 

g(rsu)=W!'S(z)+t+e(r) (8.11) 
其 中 ,W 是 输出 层 的 权 和 矩阵 ,去 =[zT,uT]T,e(z) 是 神经 网 络 的 逼近 误差 。S(. ) 为 高 斯 函数 ， 
也 是 隐藏 层 神经 元 的 传递 函数 。 
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S, (7) =exp(- a (8. 12) 
假设 : 在 最 佳 权 重 和 矩阵 B 上 存在 上 限 : 
Iwl<w,, (8. 13) 
性 质 : 高 斯 函数 有 界 : 
|Sstz7| 和 5. (8. 14) 
假设 g(x ,wu) 可 以 近似 为 : 
(2,u) =WIS(E) (8. 15) 
设计 的 观测 器 如 下 : 
[2(1) 一 At AHWTS( 人 ) 十 G(y 一 Ce) 
| (8.16) 


$=C£(t) 
为 证 明 观 测 器 的 稳定 性 ,定义 权 误 差 镀 =W 一 W 和 状态 变量 误差 去 = 全 一 zx。 根据 式 (8. 9) 和 
式 (8.16) 可 得 : 


站 人) 一 At TYWTS( 人 ) 一 Az 一 WTS(GE) 十 G(y 一 C2) 十 se(Cz) 
(8. 17) 
y=Ci(t) 
式 (8. 17) 左 边 加 上 WTS (全 ) ,并 减 去 W'S (区 ) ,可 得 ， 
(1) 一 A, 企 十 所 TS (全 ) +w(lt) 
(8. 18) 
y=Cz(t) 


其 中 ,A 二 A 一 GC,w(T)= 二 W'[S() 一 S(z)] 十 e(z)。 由 于 最 优 神经 网 络 权 重 有 界 ,所 以 
W,, 二 0, 满 足 |w() 二 WwW, 。 

2. 稳定 性 分 析 

定义 1: 权重 的 自 适应 更 新 率 如 下 : 

W=T[S()yTC— plczlw] (8. 19) 

其 中 T =T" 是 正定 矩阵 。 

定理 : 考虑 式 (8. 8) 描 述 的 一 般 非 线 性 系统 ,由 式 (8. 16) 描 述 的 观测 器 ,以 及 式 (8. 19) 描 
述 的 权 值 的 自 适 应 更 新 率 。 对 于 任何 有 界 初始 条 件 ,都 存在 适当 的 参数 G.、P 和 wo ,使 得 预想 的 
观测 器 能 够 满足 : 

(1) 观测 器 中 的 所 有 信号 都 是 一 致 最 终 有 界 的 ; 

(2) 估计 误差 收敛 到 任意 小 的 零 邻 域 。 

证 明 过 程 如 下 。 由 于 最 优 权重 W 是 一 个 常数 矩阵 ,所 以 可 得 W=0。 根据 加 权 误 差 全 = 
W 一 W 得 到 : 


W=W=T[S($)y"C —plcz |(W 填 W)] 


=T[S(Z)y'CC—p|Czl|W+wWw)] (8. 20) 
定义 2: 李 雅 普 诺 夫 函数 为 : 
L =72TPz + tr WITT!W) (8. 21) 


其 中 PP 是 正定 矩阵 。 这 里 定义 @ 为 
Q@= 一 (4ALP 十 P4Av) Ce. 229 
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其 中 Ac 是 Hurwitz 和 矩阵,Q 是 正定 矩阵 。 然 后 可 得 工 的 时 间 导 数 


一 了 2TP5 于 FT Pz +ir(WIrT WwW) (8. 23) 
将 式 (8. 18) . 式 (8. 20) 和 式 (8. 22) 代 入 式 (8. 23) ,得 到 : 
L= 一 本 05 +z PWTS(E) +iT Pw ir (W'S(E)ITCTC) Wriolcz| WW+w) 
(8. 24) 
基于 | 
tr[W TwW—W)] <w, wl— Iwlir Wis)arc™o))) <s, Icl:lzllwl 
(8. 25) 
可 得 : 
1 站 ee 了 2 1 2 
L R= Famin(Q) Nz +S,lPllzllWI+w, IPIllzl+s;, lc 1zllwl+ 
ow lcllzllwWl=elcllzllwl =Mm, (8. 26) 


其 中 ,mn(Q)>0 是 8 的 最 小 特征 值 ,可 得 : 
1 ~ Yee 
Mi =— Fhnn(Q) Nz) + lalcs, PllWl+ts, cl IwI+ 


pw» clIWI—plcl lw ) +w, IlPllzl 


1 ~ a 当下 pl Icl: 直 Icl|w,, . 
=— Bo Oal | -olcl (WW ee) + 


2plcl 
(S, |PI+S, lcl’ +olclw, > 
Ses, tol | | Ipllal (8.27) 
4olec| 
因为 
~ SnlPltSs, cl:+aelway 
-plel(lW = C8.28) 
olcl 
所 以 
1 _ ee 
Mi 去 一 二 Main(Q)| 主 | 十 zw |P|lzl+ lzl=M, 
2 4plc| 
(8. 29) 
假定 
(S, |Pl|+S, lc|:+olclw,) 
KSalPlts,lcl’ +olcl ee 
4o1c| 
可 得 
1 和 
Ms =— Amn Qil +w,lPllzl+ Klzl (8.31) 
假定 
Rt C8.32) 
Xiang E> . 
则 可 得 
i a I (8. 33) 


当 郑 之 < 一 0 时 ,z 有 界 。 如 果 选 择 观测 器 增益 G ,使 得 Ac 的 特征 值 足 够 大 , 则 4,,, (0) 的 
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值 相 对 于 2Pw, 十 2K 较 小 。 因 此 ,可 以 得 到 一 个 足够 小 的 v, 保 证 系统 的 精度 和 稳定 性 。 
注意 : .WC 和 5S(2) 都 有 界 ,o 过 0。 因 此 ,根据 式 (8. 20) 可 得 系统 的 输入 古 [S (全 ) 
ECIC 一 olczlw], 且 系统 的 状态 矩阵 一 o| cz 是 一 个 Hurwitz 矩阵 。 综 上 可 知 , 系 统 是 稳 


定 的 , 且 凡 有 界 。 


8.2.2 柔性 关节 机 器 人 控制 器 设计 

1. 问题 描述 

首先 建立 柔性 关节 机 械 臂 的 数学 模型 。 本 节 讨 论 的 单 连 杆 柔性 关节 机 械 臂 可 以 在 垂直 平 
面 上 旋转 。 它 的 关节 只 沿 转 动 方向 变形 , 连 杆 是 刚性 的 ,忽略 粘 滞 阻 尼 。 单 连 杆 柔性 关节 机 械 
臂 的 模型 如 图 8. 9 所 示 。 由 图 可 知 ,该 系统 由 两 部 分 组 成 一 一 右边 是 马达 ,左边 是 机 械 臂 ,中 
间 是 一 个 弹簧 。 其 中 ,电机 提供 的 驱动 力矩 为 &， 
电机 转动 惯量 为 了 ,电机 轴 的 转角 位 置 为 0; , 连 杆 
刚度 为 K ,机 械 臂 轴 角 位 置 为 b 。 从 连 杆 的 质心 
到 关节 轴线 的 距离 是 工 , 机 械 臂 的 质量 和 转动 惯 
量 分 别 为 M 和 工 。 电 机 轴 的 角速度 和 角 加 速度 分 


别 用 9， 和 六 表示 ; 同样 地 ,机 械 辟 轴 的 角速度 和 


角 加 速度 分 别 用 9, 和 6。 表示。 所 以 可 以 用 下 面 
的 微分 方程 来 描述 该 系统 图 8.9 单 连 杆 柔性 关节 机 械 臂 


160, 十 MgLsing, + K (9,—01)=0 


(8.34) 
JO—K(0,—0)=u 
假设 zx; ==9, ,xs 二 0,,x; 二 01 ,x4 二 91, 则 可 以 得 到 如 下 状态 方程 : 
元 1 = 
Mg 五 上 K 
9 sinrl (X13) 
J (8. 35) 
TX3 王 XT3 
二 一 了 (zi 一 zs) 十 了 


然后 ,利用 上 述 神经 自 适 应 观测 器 对 模型 未 知 的 单 连 杆 柔性 关节 机 械 臂 的 状态 变量 进行 
估计 。 

假设 机 器 人 的 角度 .电机 轴 的 位 置 和 角速度 不 能 直接 测量 。 单 连 杆 柔性 关节 机 械 臂 的 状 
态 空间 方程 如 下 : 


By 
K K ; 
一 二 人， 0 
I f sinz] 
X 一 次 天 0 lu (8. 36) 
re 天 
K K 过 
A 0 
J J f 
二 [= 0 50 O01 (8. 37) 


其 中 ， 
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X 一 2 (8. 38) 
Xa 
X 是 系统 的 状态 变量 ,Y 为 系统 的 输出 ,wu 是 系统 的 输入 。 假 定 : 
二 MegL 
FD 


SC — = 


I 7 
GD 一 了 (zi 一 ze) 
其 中 , 壹 ;= 王 [zi,z?] ,z= 二 [zx1 ,X27X3，X4j ; 则 可 以 将 式 (8. 36) 写 成 ， 
1 0 
ET a oa 
X 一 一 0 |u (8. 39) 
Di 2 | 0 
K K fu (5,) 
0 4 
3 J 
假设 f,(z,) 和 f(z,) 是 未 知 的 ,根据 式 (8. 36) 和 式 (8. 37) 可 得 : 
Z(t)=f(zr,u) 
(8. 40) 
y(t)=Czr(t) 
其 中 : 
0 1 ©@ i 0 0 
K 0 
0 0 一 0 fytis? 
eh I En 0 | 
0 © 0 1 
(oo A oe f 4(z4) a 
C=[0 0 0 0 
从 式 (8. 40) 中 提取 4z ,可 得 : 
Z(t)=Ar++g(r,u) 
| (8. 41) 


Gy 二 CS 的 
其 中 ,A 是 一 个 可 调 的 Hurwitz 和 矩阵,g (zx,u)= 二 f(x,u) 一 Ax。 由 于 g(x,u) 包 含 未 知 的 部 - 
分 ,所 以 可 以 使 用 神经 网 络 来 估计 它 ,构建 观测 器 如 下 : 


(4) 一 4 WTS(#) +G(y 一 CA) 
(8. 42) 


9CD) 一 CE(t) 
2. 控制 器 设计 
假设 电机 轴 的 角 位 置 zs 和 角速度 xz, 不 能 测量 ,使 用 8. 2. 1 节 中 介绍 的 观测 器 来 估计 zs 


和 zs。 假 定 4, 一 zs 二 Zz, 一 x4 二 1, 根据 前 述 内 容 可 知 ,如 果 | z| 二 v, 则 工 二 0。 工 在 球 
外 是 负 定 的 ,半径 wv 描述 为 {|z| 上 || 上 zl| 宇 v}。 因 此 ,1z| 在 这 个 球 中 是 有 界 的 ,通过 参数 P 和 
AC 的 合理 选择 ,使 v 保持 任意 小 的 值 。 假 设 X;， 和 zz 可 以 忽略 ,可 以 用 人 2， 和 全 ,分别 代替 
ZX3 和 zx4, 则 有 


第 8 章 “ 自 适应 控制 | 117 


之 1 一 并 2 
z2 =f2(z2) + (£3) 
(8. 43) 


La a 


Zz, = 十 了 
且 二 ye 2 

本 节 所 设计 的 控制 器 结构 如 图 8. 10 所 示 。 

由 于 单 关 节 和 柔性 机 械 臂 的 模型 是 四 阶 的 ,所 痉 
_ 以 控制 器 设计 可 分 为 以 下 4 个 步骤 。 

第 一 步 : 对 第 一 个 子 系统 

Z| 一 Za (8. 44) 

定义 期 望 轨迹 为 yx ,并 定义 第 一 个 子 系统 的 跟踪 图 8.10 ”基于 神经 网 络 的 控制 器 结构 
误差 如 下 


el 一 ZI1 一 ya (8.45) 
将 第 一 个 虚拟 控制 变量 x 定义 为 
Ta OTT EV (8. 46) 
其 中 ,ci 是 设计 常数 。zse 通过 一 个 一 阶 低 通 滤波 器 滤波 后 ,可 得 到 一 个 新 的 变量 x 作为 下 
一 步 的 期 望 变量 


TsXo 十 Tyo 一 ZodyZo(0) 一 Zoo00) 〈8. 47) 
时 间 常 数 +, 是 设计 常数 。 那 么 可 以 得 到 第 二 个 子 系统 的 跟踪 误差 
6 一 3 一 无 2 (8. 48) 
el 的 导数 为 
6 = Y= a= 下 es (zo 一 远志 和 (8. 49) 
注意 ,上 述 方程 中 存在 误差 zx 一 zzz 。 为 了 消除 误差 影响 ,定义 补偿 变量 a 为 
&i = 三 一 Cdiai 十 cz 十 (zz — rosds A1(0W=0 (8. 50) 
as 将 在 下 一 步 中 定义 。 这 样 就 可 以 得 到 补偿 变量 的 跟踪 误差 
y] 一 el 一 Qi 8,51 
ys =é2 —ay (8, 5 
第 二 步 : 考虑 第 二 个 子 系统 ,利用 RBF 神经 网 络 双 近 未 知 函 数 f(z,) 
ts = fa(F2) 十 了 2 一 W Sa (Zs) +er(F2) + 守 人 (8.53) 


其 中 ,W, 为 最 优 权重 矩阵 ; S， 是 RBF 神经 网 络 的 基 函 数 ,是 一 个 高 斯 函数 。e， (元 ,) 为 径 向 
基 神 经 网 络 的 逼近 误差 ,满足 |e,(z,)| 过 e,。 定 义 第 二 个 虚拟 控制 变量 x4 为 
kos = [WISLE2) — ce 一 cl za.] (8.54) 
其 中 ,cs 是 设计 常数 , 启 , 是 W, 的 估计 。x，。 通过 一 个 一 阶 低 通 滤波 器 滤波 后 ,得 到 一 个 新 的 
变量 x。 作为 下 一 步 的 期 望 变量 
Ts xa 一 和 id Tac (0) = 723g (0) (8. 55) 
时 间 常 数 c, 是 设计 常数 。 这 样 就 可 以 得 到 第 三 个 子 系统 的 跟踪 误差 


@s Ts Tse (8.56) 
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并 且 可 以 得 到 e, 的 导数 


=WIS,(z,) es (zs) 一 cyey —€] + [es Lvs = | (8..57) 


其 中 ,多 ,=W 一 太 , 。 注 意 , 上 述 方程 中 存在 误差 x，. 一 zy 。 为 了 消除 它 的 影响 ,定义 补偿 变 


量 ao， 为 : 
&, 一 C2Q2 les T(z Xan a»(0)=0 (8. 58) 
as 将 在 下 面 的 步骤 中 定义 。 这 样 就 可 以 得 到 补偿 变量 的 跟踪 误差 
ys =es— Qs 48597 
定义 预测 误差 为 : 
Q2NN PE 一 你 (8. 60) 
其 中 ,2; 为 
£, =W]I S, (i,) 十 了 4 十 BsasnN， 你 (0) 一 zy(0) (8. 61) 


式 中 ,6;, >0 是 一 个 设计 常量 ,选择 W, 的 更 新 规则 如 下 : 
W, =T, [Cs 十 Pasww)Sy (zs) — osW,] (8. 62) 
其 中 0 6) 和 p20 均 为 设计 常数 。 
第 三 步 : 考虑 第 三 个 子 系统 


你 ， 一 全 (8. 63) 
定义 第 三 个 虚拟 控制 变量 ziz 为 : 
Xd =—cses 了 es 5 (8. 64) 
其 中 ,cs 是 设计 常数 。ziu 通过 一 个 一 阶 低 通 滤波 器 滤波 后 ,得 到 一 个 新 的 变量 rz. 作为 下 一 
步 的 期 望 变量 。 
Wi 0 (8. 65) 


时 间 常 数 z 为 设计 常数 。 则 可 得 第 四 个 子 系统 的 跟踪 误差 如 下 : 


@4 = 一 元 4 (8. 66) 


C 


进而 得 到 e; 的 导数 为 : 
一 公 ， 到 一 一 ces — Te 四 (8 .67 


注意 : 上 述 方程 中 存在 误差 Ti 一 zid。 为 了 消除 它 的 影响 ,将 补偿 变量 ns 定义 为 : 


by 一 一 cas — Te 二 大 二 《8 (8. 68) 
a4 将 在 下 面 的 步 又 中 定义 。 这 样 就 可 以 得 到 补偿 变量 的 跟踪 误差 ; 
ys 一 e4 一 ai4 (8.69) 
第 四 步 : 考虑 到 第 四 个 子 系统 ,利用 RBF 神经 网 络 副 近 未 知 函 数 f(z,) 如 下 : 
外 一 7 十 了 =WIS,(z0) 十 et 十 了 (8.70) 


其 中 ,W, 为 最 优 权 和 矩阵 ,S, 为 RBF 神经 网 络 的 基 函 数 , 是 一 个 高 斯 函数 ,es (z, ) 为 径 向 基 神 
经 网 络 的 允 近 误差 ,满足 |e, (7z,)| 才 ei。 
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定义 最 后 的 控制 变量 为 : 
u =J[=WIS, (zi) — ce — es is.] 
其 中 ,c， 是 设计 常数 ,W， 是 W, 的 估计 。 从 而 可 以 得 到 es 的 导数 如 下 : 
és 一 人， —A =WIS (Ti) er (Ti) — cres — es 
其 中 ,万 ,= 多 , 一族 , 。 补 偿 变 量 a, 定义 为 : 
&4 =—cigs —as, ai(0) =0 
定义 预测 误差 : 
WNN 三 人 一 人 

”其 中 ,全 , 定义 为 : 
”=WIs, 4 ) 十 了 十 Plainw， 条 = (0) 


式 中 ,8, 之 0 是 一 个 设计 常量 ,这 里 选择 W, 的 更 新 规则 为 ， 
W, = LG 4 Ten ) St pr] 
其 中 ,了 二 0 >>0、.o 二 0, 它 们 均 为 常数 。 
3. 稳定 性 分 析 
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(C8, TL 


(8 72) 


(8.73) 


(8.74) 


48.795) 


(8.76) 


结合 式 (8. 35) 描 述 的 单 连 杆 柔性 关节 机 械 臂 , 式 (8. 71) 描 述 的 动态 表面 控制 器 , 式 (8. 16) 
描述 的 观测 器 ,以 及 式 (8. 19) 、 式 (8. 62) 式 (8.76) 描 述 的 自 适应 更 新 权重 率 ,对 于 任意 右边 界 
的 初始 条 件 ,存在 着 可 适应 的 参数 G、c; ,B; (i 二 1,2,3,4) 以 及 本 .Tp 、ps po4 能 够 保证 上 述 所 


设计 的 控制 系统 满足 以 下 两 个 条 件 : 
。 控制 系统 的 所 有 信号 都 是 有 边界 的 ; 
。 轨迹 误差 v;, 和 e; 可 以 收敛 到 0 的 无 穷 小 。 
证 明 过 程 如 下 。 
定义 3， i 


pp pe > 5 WT W,, 区 Ta 和 


2 ;3,4 2 ;234 


5 ZT Pz 十 Ftr (WT W) 


考虑 到 轨迹 误差 的 时 间 2 吉 刘 有 


Vi 一 6 一 4 一 一 Cel a 人 (es 一 as) 一 一 C121 十 7z 


(8, 77) 


(8.78) 


a K 
六 一 一 6 =WIiSi(z) es (Ts) — cyles —as)+ (el—al) 


和 K 
=WI!S, (元 ) 十 ey (Ts) — covs — yi ee 


i 


Vs =és — ts 一 一 cake 一 Ca3) (Bn— yd (Be 


f 


Dy 一 2 一 & =—=WiSi(z) te (zi) 一 ci(el 一 ai) 十 (es 一 os) 十 
Wi1S,(z1) 十 sy (zi) 一 ci — vs 
NT 一人 =W; 十 es (元 3 ) = BasNN 


~、 A wT Es 2 
dn = ze Wy eR) = Bany 


(C8. 79) 


K 
本 十 六 


(8. 80) 


(C831 
(8. 82) 


(8.83) 
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然后 ,可 得 李 雅 普 诺 夫 函 数 的 时 间 导 数 


4 
= 了 了 2" 十 了 TPZ 二 tr(WiT 1W) 十 Dv 十 > ,Re pe 3 (WIT;'W,) 


i=] i=2,4 i=2,4 、 


4 
一 >) (一 ci) 十 >， [viei (Fi) 十 PiaiNNei (zi) 一 PiBiGiNN —pWIWI+HL (8.84) 


1 一 1 1 一 2,4 i 
通过 观察 器 的 稳定 性 分 析 ,可 知 : 
如 果 
i A 
dmin CO) i 
则 有 
i (8. 86) 
从 而 可 得 
4 
让 二 》 ey) D) [viei (zi) + Tainves (zi) — TBidiny — pWIW] (8.87) 
i=] i=2,4 
定义 
1 ,1 i 1 2 
VX mE Ty (8. 88) 
2 i=1 2 Map 
如 果 
ee (8.89) 
Anmin(Q) Eh 
可 得 百 <V。 考 虑 如 下 不 等 式 
(元 ; ) \ 2 
ye; (Ti) 一 ci <-c( 人 一 ) :ee (8. 90) 
: 2 4c; 
S ECF NS 1 
QiNNEi (Zi;) — BiaiNnN <—B(ainy | tage (zi) (8.91) 
mT oT 4 2 
W;W, —W; W, <— Wr pee) | (8. 92) 


i 


wi | 


卫 max 
村 A (8. 93) 


2c i 2B nin 人 2 
其 中 ?Cmin 一 min[e; ] ,Brin 5 min[Bj,T .nin 年 min[T.; Jy pa -3 min[o; SY 3 max[ 卫 -: ]， 
Whax =max[ | 机上,o = max Ep ls 


假定 
1 2 De 党 Omax 2 
.94 
D 2C min 2B re 2 max (8 9 ) 
如 果 
€; (FT;) 
We | 这 (8. 95) 
2 i Cmin 


或 者 
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A (8. 96) 
“wy 20， Tuibsh 
或 者 
|m. -|> (8. 97) 
2 Omin 


则 有 声 过 V 过 0。 因 此 ,可 以 得 到 v, .ajww 以 及 在 下 列 定义 的 集合 中 被 确定 边界 的 | 多, | 。 


D E 
OO = max 
i RE 二 


) 《8. 98) 


二 E max (8. 99) 
9 sf aiNN | | aiNN | 的 i 
thy 过 (mw, ] IW, | 二 | Ps 六 -| (8. 100) 


进一步 ,去 可 由 集合 人 | 二 | 去 | 六 > 确定 边界 ， ta Dll bt 从 而 确定 该 系统 中 
的 信号 边界 。 如 果 选 择 的 c; 和 8p; 的 值 足 够 大 , 则 v; 和 ajww 的 误差 可 以 达到 无 穷 小 。 

8.2.3 仿真 

为 了 验证 该 区 A 给 定 的 机 械 臂 系统 参数 为 : 
L=1lm,M=2kg,I=2kg* m’,J=0.5kg* m’,K=10N. m/rad,g=9.8m/s’。 

参考 轨迹 为 : 


jp — 

二 =2sin (1) Fx za (8. 101) 
由 一 “ai 

仿真 结果 如 图 8. 11 一 图 8. 21 所 示 。 2.0 

首先 ,去 掉 状 态 观测 器 来 检查 控制 器 的 a 天 


性 能 。 
参考 轨道 y， 和 zys 与 实际 变量 y 和 zy。 
的 关系 如 图 8. 11、 图 8. 12 所 示 。 从 图 中 可 以 8 0.5 


看 出 ,实际 输出 y 和 xz, 可 以 准确 .快速 地 中 0 
踪 参 考 轨迹 yj 和 xys。 -0.5 

个 次 和 .xz 赴 可观 ; 并 且 全 用 洲 目 状 02 03 和 04550 
态 观 测 絮 的 估计 人， 和 3, ,而 不 是 zs3 和 zs。 时 间 /sec 


状态 变量 z 与 估计 值 42 之 间 的 关系 如 图 8.11 参考 轨迹 y, 和 实际 输出 y 之 间 的 关系 
图 8. 13 一 图 8. 16 所 示 。 从 它们 可 以 看 出 ， 
观测 器 的 状态 与 实际 系统 的 状态 一 致 .准确 且 迅 速 。 因 此 ,该 观测 器 的 跟踪 性 能 满足 要 求 。 

参考 轨道 yy， 和 zus 与 实际 变量 y 和 zx, 之 间 的 关系 如 图 8.17 一 图 8. 18 所 示 。 可 以 看 到 
实际 输出 y 和 zz 可 以 跟踪 参考 轨迹 y。 和 zu 以 及 在 不 受 状态 观测 器 作用 的 情况 下 运行 的 
仿真 。 

神经 网 络 的 权 值 如 图 8. 19 一 图 8. 21 所 示 。 可 以 看 出 ,神经 网 络 的 权 值 在 短 时 间 内 收敛 
到 最 优 值 。 总 之 ,本 节 介 绍 的 基于 神经 网 络 的 控制 器 在 模型 未 知 的 情况 下 ,能 够 使 神经 网 络 观 
测 器 获得 良好 的 跟踪 误差 的 瞬 态 性 能 。 
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ES 多 
2 
1.0 1 污 Er 
一 的 估计 值 


~ 


9 = 


“0 5 10 15 20 235 30 35 0 5 60 0 55 20025 30 35 40 45 50 
时 间 /sec 时 间 /sec 


图 8. 12 参考 轨迹 zu 和 实际 状态 变量 x, 之 间 的 关系 ”图 8.13 状态 变量 x 和 其 估计 值 *| 之 间 的 关系 


we 2 一 3 

3 一 x 的 估计 什 2.5 一 x 的 估计 什 
1.0 2.0 
人 1.5 
1.0 

4 
如 0 B05 
-0.5 0 
虽 -0.5 
-1.0 
= .S$ 1 1 1 L "se eit 一 ]| .性 
0 3 0 i 妨 5 和 息 . 梓 入 交 0 .5 10 15, 20/ 25 130 35 40: 45 
时 间 /sec 时 间 /sec 


图 8.14 状态 变量 +。 和 其 估计 值 之 间 的 关系 。 图 8.15 状态 变量 zx 和 其 估计 值 +*, 之 间 的 关系 


—— X4 


10 一 x4 的 估计 值 


人 SCI 1ST 20 25 130135 M0 特 .50 -1:00 ss 10 13 20 25 了 35 和 0 地 了 6 
时 间 /sec 时 间 /sec 


图 8.16 状态 变量 zx， 和 其 估计 值 人 , 之 间 的 关系 图 8.17 参考 轨迹 y, 和 实际 输出 y 之 间 的 关系 


1.5 —— Xn 40 
2 30 

1.0 
20 
0.5 10 
i ,人 
05 二 
—20 
ee -30 
—1.5 -40 

人 .53 10 15 20.253. 30 .35 ,给 省 , 50 0 10 20 30 40 
时 间 /sec 时 间 /sec 


图 8.18 参考 轨迹 zx,， 和 实际 状态 变量 +, 之 间 的 关系 图 8.19 神经 网 络 权 值 W， 
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0 30 3 
时 间 /sec 时 间 /sec 
图 8.20 神经 网 络 权 值 W， 图 8.21 神经 网 络 权 值 W 


8.3 ”基于 在 线 模糊 参数 整定 的 双 医 自 适应 控制 


本 节 将 介绍 一 种 用 于 Baxter 双 臂 机 器 人 的 全 运动 学 和 动力 学 模型 的 在 线 自 适应 阻尼 和 
控制 力 的 仿生 控制 器 。 

自 1965 年 引入 模糊 逻辑 理论 以 来 ,模糊 控制 作为 传统 控制 方法 的 一 种 替代 已 经 广泛 应 
用 。 模 糊 控 制 通常 应 用 于 以 下 系统 : 

(1) 没有 模型 的 系统 ,因此 不 能 很 好 地 用 数学 通 近 ; 

(2) 专家 操作 员 能 用 语言 描述 系统 的 控制 方法 。 

其 他 方法 ,如 人 工 神经 网 络 (ANNs) 可 以 用 来 对 复杂 或 未 知 系统 的 动力 学 进行 建 模 ,但 通 

需要 大 量 的 训练 数据 。 相 比 之 下 ,模糊 控制 器 可 以 根据 人 类 专家 知识 实现 ,而 人 工 神经 网 络 
则 需要 长 时 间 的 训练 。 

研究 表明 ,人 类 能 通过 调整 与 力 无 关 的 机 械 阻 抗 的 大 小 和 方向 学 习 稳 定 控制 , 即 中 枢 
神经 系统 能 够 将 不 稳定 和 误差 的 影响 最 小 化 。 控 制 器 设计 的 一 个 问题 是 设计 者 需要 设置 
任意 的 学 习 参 数 。 本 节 利 用 模糊 控制 器 将 控制 工程 师 的 调 优 经 验 转换 为 对 这 些 元 参数 的 
调整 来 解决 这 一 问题 。 当 系统 具有 复杂 不 确定 的 动力 学 特性 时 ,无 法 使 用 传统 方法 。 为 
保证 学 习 的 稳定 性 ,将 这 些 增益 先 设 为 某 一 较 小 的 值 ,然后 将 该 值 作为 增益 值 模糊 推理 的 
基准 。 


8.3.1 问题 描述 

1. Baxter 双 臂 机 器 人 

Baxter 是 一 种 双 臂 机 器 人 , 它 的 手臂 采用 一 系列 弹性 驱动 器 和 力 感 应 器 使 其 具有 主动 / 
被 动 顺应 性 ,在 靠近 人 的 操作 时 能 保证 安全 。Peter Corke 的 MATLAB 机 器 人 工具 箱 开 发 了 
一 个 完整 的 Baxter 运动 学 和 动力 学 模型 ， 人 8. 22 右 侧 所 示 。 

2. 问题 描述 

Baxter 机 器 人 的 两 只 手臂 沿 着 某 一 轨迹 移动 一 个 重 物 。 该 物体 具有 弹簧 -质量 阻尼 器 接 
触 力 模拟 的 动力 学 特性 。 此 外 ,该 物体 有 一 个 内 部 干扰 ,在 手臂 上 可 以 产生 一 个 低 振 幅 、 高 频 
的 力 ; 第 二 种 干扰 作用 于 机 械 辟 的 肘 部 ,就 像 机 器 人 与 环境 或 操作 者 接触 过 一 样 。 必 须 保 
持 最 小 的 跟踪 误差 ,以 避免 对 象 被 压 碎 或 丢弃 ; 并 且 必 须 最 小 化 控制 工作 ,以 使 系统 保持 
兼容 。 
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图 8.22 Baxter 机 器 人 (每 个 臂 上 都 有 7 个 关节 ,这 些 关节 通过 串联 弹性 驱动 器 耦合 。 
右 图 是 在 MATLAB 机 器 人 工具 箱 中 生成 的 开发 模型 ) 


质量 -弹簧 -阻尼 系统 模型 如 图 8. 23 所 示 。 施 加 于 左右 臂 的 力 Fj 和 Fg 为 


[ 文 0 1 pn 0 0 XA 
半 = [Bp “= 二 泥 : . (8. 102) 
| m m m mm ” 源 XA 
IFL| fe dal IX XA kl 

一 = 证 (8..103) 
IFR 人 > 二 一 


i 区 0 和 广 ， 分 别 是 工作 空间 中 物体 质量 的 位 置 、 速 度 和 加 速度 ,XX =[X] > 
Xa 二 [XL XkJ] 是 手臂 的 位 置 和 速度 ,而 kd 、m 和 7 分 别 是 刚度 、 阻 尼 系 数 、 质 量 和 自然 对 
象 的 弹簧 长 度 (或 半径 ) 。 


2 tN 五 
图 8.23 物体 动力 学 示意 图 (Fk 和 开 是 作用 在 左 臂 和 右 臂 上 的 力 ， 
是 由 左右 端 执行 器 的 笛 卡 儿 坐 标 位 置 和 速度 决定 的 ) 
3. 机 器 人 动力 学 
机 械 臂 的 动力 学 模型 如 下 : 
Mi (qr1)dr Cr (gL od)9L GL (gL) =7r JL (gOFL 十 rdist 


MR (grR)qR +CR(GR IR)GL + GR(qR) = TR 十 JRCdR De Ra (8. 104) 
式 中 ,g 为 机 械 臂 的 关节 位 置 M?ER” ”是 对 称 的 正定 惯性 矩阵 ,” 是 机 械 臂 的 自由 度 ; 
C(q,a)gER"” 表示 科 氏 力 和 离心 力 ; G(g)ER” 是 重力 ; TwRER"” 是 向 量 控制 输入 转 矩 ; 
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TusR SR” 是 一 个 由 摩擦 、 干 扰 或 负载 等 引起 的 力矩 ; 控制 扭矩 Tu,R 是 由 设计 控制 器 生成 
的 ,以 达到 所 需 的 运动 跟踪 和 干扰 抑制 性 能 。 力 下 、FR 如 式 (8. 103) 所 述 , 通 过 物体 耦合 两 
臂 之 间 的 动力 学 ,通过 机 械 臂 雅 可 比 矩 阵 转 化 为 关节 空间 扭矩 J ,rk ER""。 

4. 轨迹 生成 


前 臂 的 平滑 轨迹 定义 如 下 : 
人 
= 人 一 15( 芭 ) +6( 全 ] (8. 105) 


其 中 ,f(0) 和 f(T) 是 起 点 和 终点 ,TT 是 轨迹 时 间 长 度 。 左 前 辟 X7 ER" 的 期 望 任务 空间 轨 
迹 如 下 : 
| Xr = [zi (OF GIs CO CO pr CO (CONT (8. 106) 
使 轨迹 的 y 分 量 通 过 最 小 加 速 轨迹 ,而 其 他 所 有 分 量 保持 其 初始 位 置 。 右 臂 XR 的 轨迹 
定义 为 在 任务 空间 的 所 有 自由 度 中 保持 与 左 臂 的 恒定 偏 移 , 即 
XE 三 [zi Gy Pp fz 0 CG) pr py (2)]T Ce 107 
其 中 ,p“ 是 左右 操纵 器 的 末端 执行 器 之 间 所 需 的 距离 。 用 雅 可 比 矩 阵 反 解法 进行 运动 学 逆 
求解 。 
对 于 前 臂 , 定 义 X* 的 一 阶 导 数 是 微不足道 的 ,这 样 义 ?就 可 以 转换 为 关节 角 gq7 ,如 下 
所 示 : 
gr -|a; dt =|j71 cg); dt (8. 108) 
类 似 地 ,可 以 使 用 北 雅 可 比方 法 式 (8. 108) 来 计算 qkR 和 dR 。 
8.3.2 控制 器 设计 
1. 生物 模拟 阻抗 和 力 控 制 
输入 转 矩 rr 和 zr 是 左右 操纵 器 的 在 线 适 应 输入 ,定义 为 


A A 


(8.109) 
TuR 一 ZT)R Tug 区 区 


为 了 简单 起 见 , 以 下 等 式 中 去 掉 下 标 (。)， 和 (。)R (特殊 情况 除外 ) 。 参 考 转 矩 +, € R"、 
自 适 应 反馈 项 rvER” 和 zi ER” 中 ,n 表示 自由 度 。 它 们 的 定义 如 下 : 
rt, 一 M(q)gx +C(g,9)9’ +G(g)—L(t)e(t) 
=J'(g)(— Fx(t)— Kx(t)e,(t)—D, (te, (1)) (8. 110) 
浊 =T(—rt(t)—K,;(t)e;(t)—D;(t)e,;(t)) 
其 中 ,M(g) ER””.C(g,8)ER” 和 G(lg)ER” 分 别 为 惯性 、 科 里 奥 利 / 离 心力 和 重力 。L (1)E€ 
R”"*” 项 对 应 于 期 望 的 稳定 裕 度 ,并 且 丁 是 形式 为 TER”" 二 diag[11…0] 的 缩减 矩阵 ,其 中 
对 角 线 上 1 的 数量 与 肩 关 节 的 数量 有 关 。 扭 矩 r。 和 zi; 分 别 是 自 适应 任务 空间 和 关节 空间 扭 
矩 。 自 适应 规则 定义 为 : 
Or 三 人 可 | 
SK CYSQRFO es | a [De (Dé CRG C8 L117) 
SD (SOD Chel Ie, [DB Ce 0) = DC) 
类 似 地 ,在 任务 空间 中 : 
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OF,(t) =QF (Ce |, | F, |)e,()—7y,F,() 

be 四 四 C8. 1123 

SD (QDR e. | Re CV ely y Dy 

在 任务 空间 和 关节 空间 中 都 定义 了 位 置 .速度 和 跟踪 误差 : 
ej(t)=qg(t)—g (GD),er(t 一 和 CD) 一 和 (1) 
人 (8. 113) 
ye Coy ley ea 
rz rr; 是 遗忘 因子 ,表达 式 分 别 如 下 : 


GD 


y; = Ce, st Jexp( 
I OF lor Ces EY 


(8.114) 


这 


BC 
0. la, (E, | 

增益 Q, .Qk 、Qp 和 a 来 自 |t, (z) | 的 模糊 关系 ,用 作 控 制 量 的 指标 ; |e; (z) | 用 来 衡量 跟踪 绩 
效 。 类 似 地 ,任务 空间 Qs, 中 ,Qk 、Qp 是 从 任务 空间 控制 量 和 跟踪 误差 下, 和 e， 的 模糊 关 
系 导出 的 ,其 中 FF, 二 (JT) 1(g)r,。 增 益 a 适 于 改变 y 的 大 小 和 形状 ,因此 如 果 跟 踪 性 能 良 
好 , 则 遗忘 因子 很 高 ,并 且 力 度 最 小 化 。 图 8. 24 说 明了 a 的 值 如 何 改变 y 的 形状 。 


= 并 了 ( 访 | )exp 人 一 


~\ -—-0=1.0 


t a 大 小 对 ) 的 影响 
1.0 


0.2r 


0710 -0.04 


0.04 0.10 


mO 


图 8.24 a 的 值 影响 遗忘 因子 y 的 示例 ( 较 高 的 a 值 具有 较 高 的 窄 形状, 因此 当 跟踪 性 能 良好 时 ， 
控制 量 也 最 大 程度 地 降低 。 当 跟踪 性 能 差 时 ,遗忘 因子 被 最 小 化 ) 


2. 控制 增益 的 模糊 推理 
在 对 e, 和 zt, 进行 模糊 化 之 前 ,必须 首先 以 某 种 方式 对 其 进行 归 一 化 。 对 每 个 自由 度 计 
算 平均 跟踪 误差 6, ,E, ER' ,输入 扭矩 f ER” 和 输入 力 玉 ER" 。 因 此 
| S| as hss 
Ezit 人 > 


9 €,; 
trl 下 x 
人 ee (8. 115) 
| A | | 
LD i 


该 初始 数据 是 从 使 用 固定 值 增益 Q, .Qi 、Qp 和 a 的 控制 器 收集 的 数据 。 为 确保 稳定 性 ， 
可 以 将 它们 先 设置 为 某 个 较 小 的 值 , 且 不 需要 像 通 常情 况 那 样 进行 调 优 , 以 改进 跟踪 误差 或 控 
制 的 性 能 。 计 算 模 糊 系 统 的 输入 5, 、E; 、t, \F, ,将 这 些 基 线 与 系统 的 反馈 比较 ,从 而 对 一 些 
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性 能 指标 进行 识别 : 
| sey [eC | 
5 0 E(t) 
: Sn 人 Ex 
[zc Cz | Rs ya 
i ,FE, =—o—o (8. 116) 
. ， 全 


u 


结果 显示 ,对 于 所 有 的 输入 模糊 系统 ,o 值 表明 控制 器 是 执行 相同 的 固定 值 控制 器 , 值 小 
于 o 表示 一 种 进步 , 值 大 于 o 表明 性 能 更 差 。 这 里 设置 c 王 0.5, 有 效 地 正常 化 的 结果 在 0 和 1 
之 间 (1 的 结果 表明 ,目前 的 反馈 是 两 售 基线 )。 由 于 式 (8. 116) 中 生成 的 变量 没有 上 限 ,因此 
任何 大 于 1 的 输入 都 将 返回 high 分 类 中 的 最 大 隶属 度 。 这 人 允许 一 组 通用 的 输入 成 员 函 数 应 
用 于 所 有 系统 。 

控制 增益 模糊 推理 的 规则 是 利用 专家 知识 来 设置 的 ,但 在 一 般 情 况 下 : 如 果 控 制 功 耗 过 
高 , 则 增益 设置 较 低 ; 如 果 跟 踪 误 差 小 , 则 增益 设置 高 。Q 增益 的 模糊 真 值 表 ( 见 表 8. 1) 证 明 
了 这 一 点 。 和 遗忘 因子 增益 的 真 值 表 略 有 不 同 , 见 表 8. 2。 控 制 效果 和 跟踪 误差 的 变化 对 Q 增 
益 的 影响 如 图 8. 25(a) 所 示 。 可 以 看 出 ,一 般 情况 下 ,跟踪 误差 大 、 控 制 效 果 小 时 ,增益 增 大 ; 
跟踪 误差 小 、 控 制 效 果 大 时 ,增益 减 小 。 表 面 的 a 的 模糊 推理 如 图 8. 25(Cb) 所 示 。 


(a) Co 自 适应 增益 的 规则 曲面 (b) cx 的 规则 曲面 
图 8.25 规则 曲面 图 (a 值 是 基于 式 (8.116) 中 描述 的 输入 和, ) 


表 8.1 模糊 推理 控制 增益 0. .Or 、Qk ,Qi .Qp 和 Qp, 的 真 值 表 
FussPF, 


低 相等 高 

低 地 了 L 

Ex yEi 相等 5 M M 
高 l H H M 
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8.3.3 仿真 结果 


利用 MATLAB 机 器 人 工具 箱 ,对 Baxter 双 臂 机 器 人 的 全 运动 学 和 动力 学 模型 进行 仿 
真 。 其 任务 是 在 不 粉碎 或 松 开 双 手 握 把 的 情况 下 ,将 不 确定 动力 学 的 物体 沿 轨迹 移动 。 该 物 
体 的 模型 如 前 所 述 ,包括 接触 力 Fj.k 以 及 高 频 内 扰 力 Fi 和 Fik。 此 外 ,还 对 双 辟 施加 低 
频 、 高 幅 值 的 外 部 扰动 F。。 ,模拟 机 器 人 与 环境 的 相互 作用 。 内 部 和 外 部 力 Fi 和 下。 组 成 
的 扰动 力矩 rj 在 四 个 阶段 进行 了 介绍 : 在 第 一 阶段 只 应 用 F。。; 第 二 阶段 ,同时 应 用 下 。。 
和 Fiu; 第 三 阶段 仅 应 用 Fiu; 第 四 阶段 无 干扰 。 

本 节 介 绍 的 控制 器 与 类 似 的 控制 器 相 比 ,具有 Qt . ,增益 和 a, .的 固定 值 ,以 确定 模糊 扒 
理 的 增益 。 性 能 指数 7 计算 如 下 ， 


7=|" [F, CD)IOF (1) +el(t)Re, (1)]dt (8.117) 
t, 


其 中 ,Q ,RER" 为 正 对 角 扩 展 矩 阵 ,可 以 设置 +, 和 zy 的 值 来 获得 每 个 阶段 仿真 的 yx。 性 能 
指标 越 低 越 好 ,因为 其 目标 是 最 小 化 跟踪 误差 和 控制 工作 。 

在 两 种 情况 下 , 均 应 用 相同 扰动 的 定 值 控制 器 和 模糊 推理 控制 器 进行 仿真 。 对 于 Fi 振 
幅 设 置 为 10N ,频率 设置 为 50Hz; 对 于 下 振幅 设置 为 50N ,频率 设置 为 0. 5Hz。 对 象 的 动 
态 属性 设置 为 k= 二 10,4d = 二 5,m 二 1。 轨 迹 T 了 的 周期 设 为 2.4s, 所 以 在 每 一 阶段 ,手臂 将 穿 过 轨 
迹 两 次 。 其 性 能 是 通过 在 式 (8. 117) 中 相应 地 设置 i, .zy 来 衡量 的 。 此 外 ,输入 转 矩 的 二 范 数 


与 跟踪 误差 的 积分 | | r。 || dt 和 | le ,| dx 单独 进行 评价 。 


跟踪 误差 性 能 的 结果 如 图 8. 26 所 示 。 由 此 可 见 , 模 糊 推理 的 控制 增益 大 大 改善 了 跟踪 双 
臂 的 所 有 阶段 的 仿真 效果 ; 特别 是 在 第 开 阶 段 ( 扰 动力 最 大 时 ), 可 以 看 出 , 当 增益 是 模糊 推断 
时 , 右 臂 的 误差 很 小 。 从 图 8. 27 中 可 以 看 出 ,除了 第 二 阶段 外 ,所 有 阶段 的 控制 力矩 都 有 轻微 
的 改善 ,在 第 二 阶段 中 ,模糊 推理 控制 器 的 控制 效果 有 所 增加 。 还 可 以 注意 到 , 左 臂 的 输入 捏 
和 矩 明显 小 于 右 臂 ; 这 是 由 于 下 ( 右 ) 臂 不仅 要 不 断 地 调整 干扰 力 , 而 且 还 要 不 断 地 调整 前 ( 左 ) 
臂 的 不 确定 位 置 。 对 比 图 8. 26 和 图 8. 27 可 知 , 规 则 库 的 权重 偏向 于 改善 跟踪 误差 ,而 不 是 减 
少 控制 效果 。 当 任务 是 通过 轨迹 移动 一 个 动态 对 象 时 ,一 个 重要 的 考虑 因素 是 一 一 系统 遵从 
性 可 能 不 是 那么 重要 。 机 器 人 左 臂 各 阶段 的 性 能 指标 如 图 8. 28 所 示 ,将 模糊 推理 增益 控制 器 
与 固定 增益 控制 器 进行 了 比较 。 


各 阶段 跟踪 误差 


llexll 
图 


亚 
不 同 阶段 
图 8. 26 每 个 阶段 的 跟踪 误差 积分 | | ez- | dt, 分别 为 左 ( 黑 、 白 条 柱 ) 和 右 ( 黑 点 、 灰 点 条 柱 )， 
比较 模糊 推理 控制 增益 的 控制 器 与 固定 增益 的 控制 器 
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各 阶段 跟踪 误差 


350 


lell 


200 - 
I 亚 V 
不 同 阶段 
图 8.27 比较 每 个 阶段 控制 作用 的 积分 | | r， | dt 


从 图 8. 28 和 图 8. 29 可 以 看 出 ,对 控制 增益 进行 模糊 推理 的 控制 絮 在 左右 两 辟 的 性 能 上 
都 优 于 固定 增益 的 控制 器 。 必 须 指出 的 是 ,即使 在 第 二 阶段 ,所 观察 到 的 模糊 控制 器 的 控制 效 
果 有 所 增加 ,但 跟踪 性 能 的 改善 要 大 得 多 ,导致 性 能 指标 得 分 较 低 。 


左 臂 右 臂 


450 800 
400| 上 


350 
300| 
250r 
“20l 
150 
100 
50 
ee I 开 NV 人 I J TV 
不 同 阶段 不 同 阶段 
图 8.28 左 辟 性 能 指标 比较 ,模糊 自 适 应 增益 的 图 8.29 右 辟 性 能 指标 比较 ,使 用 模糊 自 适应 增益 的 
控制 器 的 指标 用 白色 表示 ， 控制 器 的 指标 用 虚线 矩形 表示 ， 
固定 增益 控制 器 用 灰色 表示 固定 增益 控制 器 用 黑白 矩形 表示 


8.4 基于 有 限时 间 收 敛 的 机 器 人 自 适应 参数 估计 与 控制 


针对 未 知 机 器 人 的 运动 学 和 动力 学 参数 ,本 节 将 介绍 一 种 提高 收敛 速度 的 机 器 人 控制 / 辨 
识 方法 。 

作为 最 有 前 途 的 技术 之 一 ,机 器 人 技术 已 经 在 工业 界 和 学 术 界 得 到 了 广泛 研究 ,并 在 现代 
制造 业 和 工 广 中 得 到 了 广泛 应 用 。 如 ABB 公司 制造 的 YuMi 机 器 人 ,Rethink 公司 制造 的 
Baxter 机 器 人 等 。 为 保证 机 器 人 动力 学 高 度 非 线性 .时 变 、 强 耦合 的 控制 性 能 ,利用 完整 的 机 器 
人 动力 学 信息 ,可 以 考虑 使 用 基于 模型 的 控制 方法 。 许 多 性 能 良好 的 机 器 人 控制 器 ,如 计算 转 和 矩 
控制 .比例 导数 增 广 控制 和 阻抗 控制 等 ,都 是 基于 模型 的 控制 ,并 已 经 成 功 应 用 于 机 器 人 控制 。 
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基于 模型 的 控制 的 前 提 是 机 器 人 的 动态 模型 必须 是 完全 已 知 的 ,因为 不 准确 的 模型 可 能 
导致 控制 性 能 下 降 ,甚至 导致 不 稳定 。 采 用 模糊 逻辑 控制 和 神经 网 络 等 基于 通 近 的 控制 方法 
可 以 学 习 机 器 人 的 动态 模型 。 然 而 ,在 这 些 基于 近似 的 控制 中 ,只 能 证 明 跟踪 误差 的 一 致 有 界 
性 ,并 不 能 保证 估计 权 值 的 收敛 性 。 这 可 能 会 导致 收敛 速度 较 慢 、 权 重 训练 过 程 较 长 ,阻碍 了 
在 实际 应 用 中 基于 近似 的 控制 。 另 外 ,牛顿 - 欧 拉 (Newton Ewler,NE) 建 模 方法 利用 正 向 和 反 
向 迭代 建立 机 器 人 动态 模型 , 建 模 精 度 高 .计算 效率 高 ,在 机 器 人 动态 建 模 中 得 到 了 广泛 的 应 
用 。 本 节 将 介绍 一 种 改进 的 NE 方法 建立 机 器 人 的 动力 学 模型 。 

在 NE 建 模 过 程 中 ,机 器 人 动力 学 参数 的 知识 对 于 建立 精确 的 模型 至 关 重 要 。 然 而 ,由 于 
复杂 的 机 器 人 机 构 和 环境 的 不 确定 性 ,这 些 参数 很 难 提前 知道 。 目 前 获取 机 器 人 动态 参数 的 
方法 有 很 多 ,如 利用 最 小 二 乘法 (Least Squares,LS) 对 未 知 的 惯性 参数 进行 辨识 ,对 机 器 人 动 
力学 辨识 ; 有 人 利用 改进 的 LS 标识 符 识别 仿 人 机 器 人 上 上 肢 的 动态 参数 等 。 虽然 有 许多 方法 
识别 未 知 的 动态 参数 ,但 很 少 考虑 估计 的 收敛 速度 。 没 有 估计 参数 的 精确 、 快 速 收敛 ,无 法 及 
时 抑制 未 知 机 器 人 动力 学 引起 的 扰动 ,可 能 导致 控制 性 能 和 瞬 态 响应 退化 。 

另 一 方面 ,机 器 人 控制 中 普遍 存在 运动 学 不 确定 性 。 针 对 具有 不 确定 运动 学 和 引力 的 机 
器 人 ,在 不 使 用 机 器 人 雅 可 比 矩 阵 的 情况 下 ,有 人 提出 一 种 自 适 应 设 定 值 控 制 方案 。 而 对 于 机 
器 人 运动 学 和 动力 学 未 知 的 问题 ,可 以 使 用 结合 自 适应 雅 可 比 矩 阵 和 逼近 的 机 器 人 神经 网 络 控 
制 方案 。 上 述 自 适 应 方案 虽然 保证 了 跟踪 控制 的 性 能 ,但 只 获得 了 估计 参数 的 有 界 性 ,而 不 能 
保持 估计 参数 与 真实 值 的 收敛 性 。 由 于 通过 估计 参数 的 收敛 性 可 以 获得 更 好 的 控制 性 能 ( 尤 
其 是 瞬 态 性 能 ) ,因此 更 需要 快速 \ 准 确 地 估计 机 器 人 的 运动 参数 ,而 不 是 其 有 界 性 。 

8.4.1 机 器 运动 学 和 动力 学 建 模 

本 节 讨论 机 器 人 的 运动 学 建 模 和 动力 学 建 模 ,其 中 ,机 器 人 运动 学 将 使 用 Denavit- 
Hartenberg(D-H) 方 法 建 模 ,机 器 人 动力 学 模型 将 使 用 牛顿 - 欧 拉 公式 建立 。 

1. 机 械 辟 运动 学 建 模 

设 定 机 器 人 末端 执行 器 的 位 置 向 量 x €E 恨 ”和 关节 向 量 gER"N,N。 是 任务 空间 的 自由 
度 ,N 是 关节 空间 的 自由 度 。 机 械 臂 的 正 向 运动 学 描述 如 下 : 


工 一 inae(9) (8.118) 
对 fine 求 偏 微分 ,可 以 得 到 关节 速度 4 和 末端 执行 器 速度 之 之 间 的 关系 : 
9f yine(q) 
二 0 =J(g,0)gd (C8: 119) 


9g 
表 8. 3 为 使 用 牛顿 - 欧 拉 法 建 模 时 ,对 应 的 运动 参数 和 惯性 参数 。 


表 8.3 NE 建 模 中 的 运动 学 参数 和 惯性 参数 


执行 器 在 关节 i 处 的 扭矩 
第 i 坐标 系 的 角速度 

第 i 坐标 系 的 角 加 速度 
连 杆 i 的 线 速 度 
连 杆 i 的 线 加 速度 
由 连 杆 i 的 运动 引起 的 关节 i 处 的 力 
由 连 杆 i 运动 引起 的 关节 i 处 的 扭矩 
连 杆 i 的 惯性 参数 


p: ER 
yp; ER 
vi ER 
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其 中 ,09ERR* 为 常数 ,表示 运动 学 参数 ,h 表示 运动 学 参数 个 数 ,J (dg,b)E 及 ”是 机 器 人 雅 
可 比 矩 阵 。 
基于 以 下 性 质 和 假设 ,对 参数 进行 估计 。 
性 质 1: 雅 可 比 矩 阵 J(c,0) 可 线性 参数 化 为 ， 
J(g,0)9 =R(g,9)0 (8. 120) 
其 中 ,R(g,9)ERRN* 是 雅 可 比 和 矩阵 的 回归 函数 。 
假设 1: 机 械 辟 的 雅 可 比 和 矩阵 是 满 秩 的 ,机 器 人 在 估计 过 程 中 能 够 被 很 好 地 控制 ,避免 奇 
蜡 点 5 
2. 机 械 辟 的 动态 建 模 
利用 牛顿 - 欧 拉 法 建立 机 器 人 的 动力 学 模型 。 建 模 过 程 由 两 个 递归 组 成 : 前 向 递归 和 后 
向 递归 ,其 推导 过 程 如 下 。 表 8. 3 给 出 了 建 模 过 程 中 用 到 的 以 下 参数 : 
pi = 人 
yi; 3 RO 十 Ra X id; 十 愉 : 
= RD PW Wd (8. 121) 
a 一 mi +y;X ME: ty; X (yy; X m,, ) 
12 ci =1,y, 十 y; X GT 一 区 Xm 
其 中 ,z; 为 关节 旋转 轴 的 向 量 , 如 >; 王 [0,0,1] ,8 一 0) 十 内 XIR 是 旋转 变换 矩 
阵 ,; “PERR 为 变换 向 量 , 则 式 (8.121) 对 m; mj 、1; 的 线性 参数 化 可 表示 为 : 


ca = [na 的 
二 万全 EC(%,) +L(Y ECY,) 
Liby LO ey 0 3 
3 yi y; 消 ; CCT 
A (8. 122) 


其 中 ,4; 是 一 个 具有 适当 维 数 的 矩阵 ,多 一 [mi ,zi ,CCT， )]" 是 一 个 未 知 惯性 参数 的 矢量 ， 
友人 天) 一 [三 为 链 惯 性 矢量 (7) 和 五 (9) 定 义 为 


0 一 人 hy 
L(y)= 峭 - 0 = 
By We 0 


EW | WB 0 WB We 0 
I Ws 0 Wy Ws 
需要 注意 的 是 ,5;; 是 关节 的 力 还 是 力矩 完全 取决 于 连 杆 i 的 运动 。5 可 以 通过 结合 每 个 


N 
连 杆 的 力矩 以 下 述 方式 得 到 , 即 5; = 2 5; ,其 中 


PiyR SR 
和 光 喇 | 
itR 0 
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因为 r+; = 二 [z7 0]75; ,考虑 式 (8. 122) ,可 以 得 到 如 下 线性 化 参数 化 的 牛顿 - 欧 拉 公式 


SH Sl “™ DN 2， 
0 S22 、… 92N 2， 
z 一 90 = . 和 和 (8. 123) 
0 0 SNN| IB 


其 中 ,H，, ry 。 特别 地 , 互 ; =A, ,53 3 [z; 0] "7 。 
8.4.2 ”有 限时 间 参 数 辨识 
本 节 将 讨论 基于 有 限时 间 辨 识 的 方法 来 获取 未 知 的 机 器 人 运动 学 和 动力 学 参数 。 
1. 运动 学 参数 的 辨识 
图 8. 30 为 有 限时 间 运 动 学 估计 策略 示意 图 。 


有 限时 间 收 敛 参数 估计 
gq,g 雅 可 比 和 矩阵 ” | R(g, 9)6 被 估计 参数 者 hg) 
回归 矩阵 R(g, 9 
K 


(OQ)g=R(q, 9)0 


有 限时 间 估 计 


机 器 人 闭环 系统 位 置 控 末端 执行 器 


制 信号 位 置 x 


记 机 器 人 控制 器 人 8 


图 8.30 有 限时 间 运 动 学 估计 策略 示意 图 


估计 未 知 的 运动 学 参数 0 时 ,要 保证 估计 值 在 限定 时 间 内 收敛 。 结 合式 (8. 118) 、 式 (8. 119) 
和 式 (8. 120), 有 


需要 注意 的 是 ,末端 执行 器 速度 是 通过 外 部 传感器 测量 的 ,外 部 传感器 可 能 对 噪声 敏 
感 。 为 避免 这 一 情况 ,在 式 (8. 124) 的 两 侧 采用 稳定 的 线性 滤波 器 (*)7 一? i 。) ,如 下 式 
1 
所 示 
R/,(q,9)0 = (8. 125) 


其 中 ,R,(g,9)ERR"* "是 滤波 后 的 回归 矩阵 ,之 为 滤波 后 的 速度 ,满足 如 下 等 式 
|/ Cg) + RG,9) 一 RCq 7 人) Rp-o =0 
(8. 126) 
[ 溉 池 二 zr= Zrlte0o 一 0 
其 中 ,li 是 一 个 正常 数 , 式 (8. 126) 中 可 以 用 
位 置信 息 。. 下面 介绍 两 个 定义 。 
定义 1: 如 果 存 在 T>0,e>0 使 得 | RrcDTRrCr)dr 三 eI ,Vt 宇 0, 则 向 量 或 矩阵 函 


一 一 代替 之/ 。 因 此 ,在 估计 过 程 中 只 需要 使 用 


1 
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数 尺 ,是 持续 激励 的 (或 满足 PE 条 件 ) 。 
定义 2: 如 果 存在 T. ,T。 和 e>>0 使 得 | ，Rj(r)TRj(r)dr >sT, 则 有 界 向 量 或 矩阵 


函数 Rj 是 区 间 [LT, 一 To。,T。] 上 的 间断 激励 (或 满足 IE 条 件 ) 。 

注意 : PE 条 件 虽 然 是 保证 估计 参数 收敛 的 必要 条 件 , 但 是 该 条 件 并 不 容易 满足 。 与 PE 
条 件 相 比 ,IE 条 件 只 需要 在 一 段 时 间 内 保持 ,这 大 大 降低 了 要 求 。 

引入 一 个 辅助 矩阵 DE 了 及 "““ ,两 个 辅助 向 量 TE 了 及 和 PE 有 RN 如 下 : 


| 一 一 9 DH+HRr(q,a) Rroa) D(0) =0 


We 
T=—9 看 Rr(q  ——— NO) =0 (8. 127) 


PP 二 大功 
其 中 ,9 是 一 个 正 的 常数 ,表示 滤波 矩阵 的 遗忘 因子 ,这 是 为 了 保持 鲁 棒 性 和 收敛 速度 之 间 的 
平衡 。 
对 式 (8. 127) 两 边 同 时 积分 ,得 到 D 和 7 的 解 如 下 : 
ie 一 | OR Gr TR gr) dr) ) dr 
, Ee (8. 128) 
[Tw 一 | NR) ,Gr)) 一 


dr 
从 PE 条 件 的 定义 ,可 以 得 到 | RerDRyG)dr > sT, 也 相当 于 | ，_R7Cr)RyrCr)dr = 
el], 其 中 ,>>T>0。 因 此 ,如 果 尽 r 满足 PE 条 件 , 则 | _R7TcDRyGDdr 艺 ET 对于: 思 T>0 


恒 成 立 。 然 后 ,考虑 积分 区 间 rE€[zt 一 T ,tj]。 
因为 上 一 > 委 人 ,根据 指数 函数 的 单调 性 可 得 e ?7 三 e 和 过 0。 从 而 可 以 推导 出 


ed-DRTCr)RrGr)dr >|: ee2TRTCr)RrCr)dr > eel, 此 外 ,| e ?467RT(Gr)R Cr)dr > 
一 t 一 人 0 
| EDRICr)Rylr)dr 对 于 所 有 的 上 之 T>0 恒 成 立 。 由 上 述 分 析 得 了 = 


| >RFGrD)RrCr)dr > eo] REOR, Gr)dr 宇 e“ el。 这 意味 着 D 是 正定 的 , 且 其 


最 小 特征 值 满足 4,, (D)>c,c 一 e ”Te。 
比较 式 (8. 127) 中 D(z) 与 T(z) 的 结构 ,根据 式 (8. 125), 可 以 得 到 D=T。 因 此 ,PP 可 以 写 
成 P=D90 一 D9 二 DO。 这 意味 着 PP 包含 估计 参数 的 误差 信息 0 
P 
i 3 
lo (P =0) 
是 规定 的 、 正 的 常数 。 注 意 ,在 运动 参数 的 估计 中 ,假设 末端 执行 器 位 置 x 和 关节 变量 g 是 
有 界 的 ,并 且 可 测 , 则 可 以 通过 机 械 臂 的 内 部 位 置 控制 器 来 实现 。 
定理 1: 考虑 机 器 人 运动 学 模型 即 式 (8. 120), 假 设 Ri (do,a) 满 足 正 条件 


RICDRj(r)dr 之 el，T,、T。 和 为 正 的 常数 。 然 后 ,根据 参数 估计 定律 即 式 (8. 129) ， 


运动 参数 的 估计 误差 在 有 限时 间 t。 内 收敛 到 原点 ,满足 t, 志 上 0(0) Aw (TT 1)/o 二 T,,o 
是 一 个 正 值 ,Mus(。) 是 矩阵 最 大 的 特征 值 。 


(8. 129) 
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2. 动态 参数 辨识 
本 小 节 将 介绍 一 种 自 适应 估计 方法 来 识别 未 知 的 动态 参数 ,具体 方案 如 图 8. 31 所 示 。 


机 器 人 控制 器 
控制 输入 


惯量 参数 估计 $， 
类 守 二 
有 限时 间 ”关节 力矩 r | 有限 时间 人 
参数 估计 回归 矩阵 8 估计 算法 


图 8.31 基于 有 限时 间 收 敛 的 机 器 人 控制 


(1) 辅助 矩阵 设计 , 构造 一 组 辅助 矩阵 进行 动态 识别 ,如 下 所 示 ; 
1 U(0)=0 
B=—6B+S'rt U(0)=0 (8. 130) 
E=B—US 
其 中 ,6 是 规定 的 正 的 常数 ,可 以 推导 出 和 B 的 解 UG1) =| s ?7STC0)S(r)dr,BC) 一 
| esT orar. 
注意 : 在 估计 中 ,PE 条 件 可 能 不 满足 ,这 是 因为 U(t) 可 能 不 是 满 秩 。 为 了 解决 这 一 问 
题 , 这 里 使 用 SVD-MR 方法 。 
(2) SVD-MR: 对 于 一 个 普通 的 机 械 臂 ,未 知 的 惯性 参数 可 以 分 为 三 组 , 即 参 数 绝对 可 识 
别 .参数 不 可 识别 以 及 参数 识别 为 线性 组 合 。 其 主要 思想 是 通过 以 下 3 个 步骤 建立 最 小 惯性 
参数 集 。 
@ 消去 UU 的 零 元 素 列 。 
@ 重新 组 合 U 中 的 线性 别 列 ,然后 消除 对 关节 扭矩 没有 影响 的 列 。 
@ 重新 安排 和 选择 @ 惯性 参数 。 
假设 局 矩阵 的 rank(U) 王 9 ,对 和 矩阵 U 进行 奇异 值 分 解 , 有 


GE 
U=VAY =V CB 3 
(oe 了 


其 中 ,VER* ”5 和 Y 一 [YT,YT]TER2 “5 是 正 交 和 矩阵 , 瑟 E 了 “是 一 个 主 元 素 的 矩阵 ,5， 
是 表示 U 的 维 数 的 常数 。Y1 ER" 和 了 ER"“ 是 子 矩 阵 。 
根据 式 (8. 131), 有 BG) UG)$ 二 V3Y?$ 一 | ee 2STCrDrdr。 进而 可 以 推出 矩阵 


Yi 和 和 矩阵 了 , 中 的 零 行 不 会 影响 B (2) 的 输出 。 因 此 ,相应 的 惯性 参数 可 以 归 为 不 可 识别 参 
数 。 另 一 方面 ,由 于 [Y1,Yz?]- 是 一 个 正 交 矩阵 ,因此 Y; 中 的 零 列 的 子 空间 对 应 于 Y1 中 线 
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性 独立 的 列 的 子 空间 ,这 意味 着 可 以 通过 Y 中 的 零 列 找到 绝对 的 惯性 参数 B。 然 后 ,将 剩余 
的 惯性 参数 分 组 为 线性 可 识别 参数 。 

对 三 组 惯性 参数 进行 分 类 后 ,还 需要 获得 线性 可 识别 参数 B。 的 组 合 形式 。 这 里 通过 消 
去 矩阵 U 中 与 不 可 识别 参数 和 绝对 可 识别 参数 对 应 的 列 ,设计 一 个 增 广 矩 阵 U, ,假设 U, 的 
秩 为 rank(U, ) 二 6,, ,然后 在 U, 上 进一步 采用 奇异 值 分 解 ,具体 如 下 : 


, > 
U, =V,A,Y, 一 V， i (C8. 182» 
0 0 YY, 


其 中 ,了 ER 是 一 个 对 角 和 矩阵 ;了 ,1 ER 和 了 E 了 ii 是 子 矩 阵 , 且 5,s 是 
表示 U, 的 维 数 的 常数 。 和 矩阵 Y,, 的 行 (和 @B。 中 相对 应 的 元 素 ) 可 以 重新 安排 如 下 


2 5 
到了 | PUD | i (8. 133) 
7 2， 


其 中 ,P 是 一 个 置换 矩阵 ,用 来 保证 YD， 为 正则 矩阵 ,GE 及 2” 和 $, ER” 是 子 向 量 。 
5. 左 乘 P' 如 式 (8.133) 右 边 所 示 , 可 以 推出 线性 可 识别 参数 B' 的 具体 形式 8' = 二 @| 一 65，， 
8=Y ,Y2 是 线性 组 合 的 系数 。 然 后 ,可 以 通过 把 B, 添加 到 B' 中 得 到 @, 一 [6B ,8B']', 通 
过 选择 S 中 对 应 的 列 ,可 以 得 到 相应 的 回归 矩阵 S, 。 因 此 ,牛顿 - 欧 拉 方程 即 式 (8. 123) 可 以 
重建 为 r*=S,G@, 。 需 要 强调 的 是 : 惯性 参数 的 重新 组 合 不 会 改变 关节 扭矩 的 计算 结果 , 即 z= 
S,G, 王 SG ,因为 S 中 删除 的 参数 和 相应 的 列 不 影响 关节 力矩 。 
(3) 有 限时 间 估 计 设 计 : 这 里 将 介绍 另 一 种 可 替代 的 有 限时 间 自 适应 方法 来 估计 参数 
B,。 首 先 定义 一 个 辅助 矩阵 8 ,其 中 8 的 时 间 导 数 为 : 
0O=650—0S1S,0, Q(0)=Qi 三 0 (8. 134) 
且 6>0 为 一 个 正 的 常数 。 注 意 ,存在 以 下 矩阵 等 式 , L001 一 O00 二 QO 一 0. 
式 (8. 134) 两 边 同 时 乘 以 O ! 并 考虑 上 述 等 式 , 有 : 
0 00 ”= 一 0 00 =60 00 
一 一 0 0S1S.00 '=60 一 S1S (8. 135) 
则 一 阶 微分 方程 式 (8. 135) 的 解 为 Q (1) =e “Qo 十 | en sro)s, ar, 这 里 用 到 了 
初始 条 件 Q。 = Q (0) 字 0, 选 择 Q。 = ,7 是 一 个 正 的 常数 。 因 为 WU.= 
en srTG)s,(r)dr, 有 QC =Le Yt 
然后 ,用 奇异 值 分 解法 求解 矩阵 M ,得 WU 二 YA》 ,其 中 是 一 个 正 交 和 矩阵 , 它 的 列 向 量 
是 WU.U! 的 特征 向 量 ,是 一 个 正 交 和 矩阵 , 它 的 列 向 量 是 WU. 的 特征 向 量 , A 是 一 个 形式 为 
4. =diag(ai,…,a,) 的 对 角 矩 阵 。 注 意 到 >. 和]7. 是 西 矩 阵 (Unitary Matrix) ,所 以 YY = 了 I 
和 2 37: = 工 恒 成 立 。 


0=Le™ G+ U1 
=[V.(A.+e yl) y 
=Y. A+e 1) Yr (8. 136) 
因为 y. 满 足 y-' 一 7 ,所 以 可 以 得 到 7 二 AY。 对 式 (8.159) 两 边 同 乘 以 U. ,得 到 
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QU =Y. Ate yu. 
一 At+e rl)! AY 
Q1 Qn i 
di i (8. 137) 
2 bg 人 (二 ey | 9 
需要 注意 的 是 ,在 上 述 方程 中 ,4. 是 一 个 对 角 和 矩阵 = 
定义 一 个 辅助 项 下 (上 ) 如 下 : 


一 0 


一 6 
F() 一 37 di 人 —— (8. 138) 
i y. lag 1 十 e 7 a, 二 +e wr ) 
则 式 (8. 137) 可 以 写成 如 下 形式 : 
Q(t) W(t)=I—F(t) (8. 139) 
= 
注意 ,对 于 非 零 特征 值 a; 小 ”oo 时 一 一 渐 - 效 一 项 软 敏 于 0。 
ER 六 
$,=6,—$,=[QU+F]S,—$, (8. 140) 


现在 估计 误差 可 以 用 式 (8. 140) 表 示 ,8. 4. 4 节 将 用 式 (8. 140) 设 计 自 适应 律 。 
8.4.3 ”有 限时 间 自 适应 控制 器 设计 
本 节 以 动态 模型 和 有 限时 间 辨 识 为 基础 ,在 动力 学 具有 不 确定 性 的 情况 下 ,控制 机 器 人 跟 
踪 期 望 的 轨迹 。 机 械 臂 的 动力 学 模型 可 以 描述 为 : 
M(g)g+C(g,9)9+G(g)=S,(g,9,9)®B, =r (8. 141) 
其 中 ,gq ER 腿 ” 是 关节 位 置 向 量 ,M (gq)ERN*N 是 对 称 惯性 矩阵 ,C(g ,9)9 ERRV 表示 为 科 里 奥 
利 力 和 向 心力 矩阵 ,G(q)E 取 ”> 是 重力 ,rERRN 是 关节 力矩 。 
在 设计 控制 器 之 前 ,引入 两 个 误差 信号 如 下 : 
a (8. 142) 
其 中 ,qs 为 关节 位 置 参 考 轨迹 ,而 9, 是 速度 参考 轨迹 ,定义 为 9, 二 4 十 Xe, ,4 是 一 个 正 的 常 
数 ,将 定义 代入 式 (8. 142) ,e, 可 改写 为 e, 二 ,十 Xe,。 
对 机 器 人 的 控制 基于 以 下 特性 和 假设 。 
性 质 2: 矩阵 M(g) 是 对 称 且 正 定 的 ,而 矩阵 M(g) 一 2C(gq ,6) 是 斜 对 称 的 。 
假设 2: 关节 位 置 参考 轨迹 gq, 及 其 导数 4 是 连续 有 界 的 。 
将 式 (8.142) 代 入 式 (8. 141) 中 ,可 得 


M(g)é,+C(g,d)e, =r—S,(9,,9,,9,9)8, (8. 143) - 
假定 控制 信号 如 下 : 
r 一 一 Kie, 十 $S. (oo,q)@. 十 r， (8. 144) 
rt, 为 鲁 棒 项 ,定义 如 下 : 
| 二 ES (es 天 0) 
t, 一 | e, | 
0 (e, 一 0) 
其 中 ,K， 和 K, 是 选 定 的 正 控制 增益 。 将 控制 器 式 (8. 144) 代 和 人 控制 器 式 (8. 143) ,可 得 误差 
动力 学 表达 式 为 : 
| M(g)é,+C(g,g9)e, =—Kie, +S, 9,,0,0,9)$, tt, (8. 145) 


设计 自 适 应 控制 律 如 下 : 
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Fe $, — QB. 会 | 
和 =By (sie, 一 一 | (| 8. 一 0B8. | 0) 
6 | 8,—QB. | (8. 146) 


间 人 (| 全. 一 0B .| =0) 

其 中 ,了 是 一 个 正 的 常数 , B. =| er sr rar. 

定理 2: 考虑 有 控制 器 式 (8. 144) 和 FT 自 适 应 律 式 (8. 146) 的 机 器 人 动态 模型 式 (8. 164) , 利 
用 生成 的 激励 轨迹 ,可 以 推导 出 跟踪 误差 收敛 到 原点 的 一 个 小 邻 域内 ,估计 误差 $B, 在 有 限时 
间 内 收敛 于 一 个 以 零 为 中 心 的 小 的 紧 集 内 。 

证 明 过 程 见 8. 4.5 节 。 

8.4.4 仿真 与 实验 

为 了 验证 算法 的 有 效 性 ,在 7 自由 度 Baxter 机 械 辟 (如 图 8. 32 所 示 ) 上 进行 仿真 和 实验 。 

1. 运动 学 有 限时 间 估 计 与 验证 

(1) 仿真 设置 : 对 于 转动 关节 链 , 采 用 D-H 参数 
建立 运动 学 模型 ,如 表 8. 4 所 示 , 标 称 值 分 别 为 a = 
0.069yas =0. 039,as =0r O01 ds = 0.300853ds = 
0. 37429,d; 二 0. 22953。 假 设 这 些 参 数 未知 , 将 使 用 
FT 适应 律 式 (8. 129) 进 行 估计 。 在 运动 估计 过 程 er 
中 ,Baxter 机 器 人 会 在 位 置 控 制 模块 中 运行 ,使 得 每 图 8.32 Baxter 机 械 展 
个 关节 的 速度 稳定 上 且 有 界 。 


表 8.4 BAXTER 机 器 人 左 臂 的 运动 学 D-H 参数 


1 gi | 0 

2 Ve 0 一 x/2 
3 | gs | /2 
4 gs | 5 | | 一 r/2 
5 qs ds 0 T/2 
7 | x/2 


令 Baxter 机 器 人 跟踪 参考 轨迹 qu 一 aicos(0. 3t 十 Dr), 其 中 al 一 ay 一 0.1,a3 一 0 一 
0 3 = = 0 = 0 =0Mb 0 I 0. 0300. be = 0 be ie 
0.2,c, 二 0.15,cs 二 cy 二 一 0.1,cs 二 co 二 cy 二 0, 在 0~~12s 和 18 一 24s 期 间 ,参考 轨迹 是 持续 变 
化 的 ,在 12 一 18s 内 保持 不 变 。 增 益 设 置 为 9==0.015, 厂 二 0.025,11 二 0.01。 选 择 估计 参数 的 
初 值 为 8(0) 一 [0.05,0.05,0.05,0.2,0.19,0.1]-I。 

(2) 仿真 结果 : 运动 估计 结果 如 图 8. 33 一 图 8. 36 所 示 。 图 8. 33 为 满足 IE 条 件 的 机 器 
人 关节 速度 示意 图 ,可 以 看 到 ,0,; 在 0 一 12s 和 18 一 24s 内 持续 变化 ,而 在 12 一 18s 保持 不 变 。 
参数 估计 性 能 如 图 8. 33 所 示 , 跟 踪 误 差 如 图 8. 35 所 示 。 从 图 8. 34 可 以 看 出 ,所 有 估计 的 
D-H 参数 ( 实 线 “-”) 都 收敛 到 它们 的 真 值 (虚线 “--”) ,收敛 速度 很 快 。 即 使 在 输入 信号 间断 期 
间 ,该 算法 仍 能 保证 估计 性 能 。 相 比如 图 8. 34 所 示 的 自 适应 梯度 下 降 法 ,该 算法 的 估计 性 能 
在 收敛 速度 和 精度 上 都 有 较 大 提高 。 图 8. 36 为 辅助 矩阵 D 的 最 小 特征 值 分 布 图 ,可 以 看 出 ， 
D 的 最 小 特征 值 在 1 一 12s 和 18 一 24s 期 间 不 断 上 升 , 在 24s 左右 达到 峰值 ,这 验证 了 IE 条 件 
的 满足 。 同 时 表明 ,该 算法 可 以 有 效 地 工作 在 区 间 激 励 输入 下 。 
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关节 位 置 q/rad 


时 间 /sec 
图 8.33 满足 IE 条 件 的 输入 信和 号 


0245 15 20 25 


0 103 22030 和 50 60 70 80 90 100 
时 间 /sec 


图 8.34 ”估计 参数 在 区 间 时 间 内 的 收敛 性 
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图 8.35 跟踪 误差 
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Amin(D) 
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图 8.36 辅助 矩阵 了 的 最 小 特征 分 布 图 
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(3) 和 鲁 棒 性 验证 与 实验 : 为 了 进一步 证 明 上 述 估计 算法 的 有 效 性 ,这 里 通过 在 系统 模型 
中 添加 未 知 的 外 部 干扰 来 进行 鲁 棒 性 测试 。 在 一 些 实际 应 用 中 ,测量 值 的 不 确定 性 是 不 可 避 
免 的 。 例 如 ,基于 摄像 头 的 目标 跟踪 系统 很 容易 受到 环境 噪声 的 影响 。 在 对 比 研 究 中 ,将 不 同 
强度 的 白 噪声 加 入 到 末端 执行 器 的 测量 信号 里 进行 测试 。 选 择 外 部 干扰 为 d(t) 二 ow (1), 其 
中 c 为 噪声 强度 ,w(t) 是 高 斯 白 噪 声 , 大 小 为 14BW。 为 了 实现 公平 比较 , 自 适 应 估计 中 的 参 
数 选 择 相 同 , 机 器 人 跟踪 相同 的 轨迹 。 仿 真 结果 如 图 8. 37 一 图 8. 42 所 示 。 末 端 执行 器 位 置 
上 的 扰动 如 图 8. 37、 图 8. 39 和 图 8. 41 所 示 ,其 中 5 分 别 为 0.005、0.01 和 0.02。 从 图 中 可 以 
看 出 ,末端 执行 器 上 添加 的 最 大 偏差 分 别 为 十 2cem、 士 4cem 和 士 6cm。 参 数 估计 性 能 如 图 8. 38、 
图 8. 40 和 图 8. 42 所 示 。 从 图 中 可 以 看 出 ,即使 存在 外 部 扰动 ,三 组 参数 的 收敛 速度 和 精度 都 

令 人 满意 。 


——di(m) 
0.10 

| ----di(m) 

d 

0.05 SR 

4 
各 0 
_0.05 
_0.10 
20 40 60 80 100 


时 间 /sec 
图 8.37 co 二 0.005 在 末端 执行 器 的 位 置 添 加 扰动 


估计 参数 


时 间 /sec 
图 8.38 ”扰动 c= 二 0.005 时 运动 估计 性 能 


——di(m) 
----ds(m) 
—d3(rad) 
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图 8.39 ”在 末端 执行 器 的 位 置 添加 扰动 (oc 二 0. 01) 
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图 8.40 ”存在 扰动 时 运动 估计 性 能 (so 二 0. 01) 


时 间 /sec 
图 8.41 在 末端 执行 器 的 位 置 添加 扰动 (sc 二 0. 02) 


估计 参数 


时 间 /sec 
图 8.42 存在 扰动 时 运动 估计 性 能 (o==0. 02) 


此 外 ,以 Baxter 机 器 人 的 左 臂 为 实验 对 象 进行 了 进一步 实验 ,如 图 8. 32 所 示 。 在 实验 
中 ,机 器 人 被 命令 跟踪 一 组 如 图 8. 43 所 示 的 正弦 和 余弦 轨迹 。 估 计 结 果 如 图 8. 44 所 示 。 从 
图 中 可 以 看 出 ,大 部 分 估计 参数 都 接近 标 称 值 ( 即 虚线 ) ,收敛 性 能 较 好 ,从 而 验证 了 该 算法 的 
鲁 棒 性 。 

2. 自 适应 有 限时 间 动 态 估 计 与 控制 

前 文通 过 数值 实验 进一步 验证 了 动态 参数 估计 的 有 效 性 。 用 NE 方法 对 Baxter 机 器 人 
的 初始 建 模 ,其 中 包含 70 个 惯性 参数 。 采 用 模型 降 维 方法 之 后 ,将 这 些 参数 重新 组 合成 43 个 
参数 ,其 中 22 个 为 绝对 可 识别 参数 ,21 个 为 线性 组 合 之 后 可 识别 的 参数 。 利 用 传 里 叶 展 开 保 
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关节 角度 4 


时 间 /sec 
图 8.43 关节 角 的 轨迹 


估计 参数 


0 10 20 30 40 50 60 
时 间 /sec 


图 8.44 运动 估计 性 能 


证 激发 条 件 ,得 到 周期 性 的 持续 激励 信号 944 、44 .qu 。 
选取 控制 增益 为 k= 二 diag[320,185,40,42,5,10,0. 38] ,ks 二 diag[26,19,3,5,0.4,0.75， 
0.03] ,选择 自 适应 律 增益 为 Pu 二 20 和 6 二 2, 初 始 参 数 @, 为 零 , 设 定 初始 状态 7,9 ,gq 为 零 。 
估计 结果 如 图 8. 44 一 图 8. 53 所 示 ,其 中 图 8. 45 一 图 8. 51 为 对 机 器 人 关节 的 跟踪 性 能 ， 
图 8. 52 和 图 8. 53 为 参数 估计 性 能 。 从 图 中 可 以 看 出 ,利用 本 节 介 绍 的 有 限时 间 估 计算 法 ,机 
器 人 能 够 很 好 地 跟随 参考 轨迹 ,并且 获得 了 令 人 满意 的 瞬 态 性 能 。 


0.5 


一 FT 
--- 自 适应 控制 


跟踪 性 能 q/rad 
5 


旺 _0.4 4 
: --- 自 适应 控制 


“0 5 10 15 20 “0 5 10 15 20 
时 间 /s 时 间 /s 


图 8.45 关节 1 跟踪 性 能 图 8.46 关节 2 跟踪 性 能 


为 进一步 验证 算法 的 有 效 性 ,分 别 基 于 PID 控制 器 、 自 适应 控制 器 和 所 设计 的 控制 器 进 
行 了 比较 研究 。 比 较 结果 如 图 8. 45 一 图 8. 52 所 示 。 由 图 可 知 ,虽然 三 种 类 型 的 控制 器 均 可 
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获得 稳定 的 跟踪 性 能 ,但 PID 控制 (用 虚线 “--” 表 示 ) 的 跟踪 误差 较 大 ,有 限时 间 方 法 的 控制 性 
能 在 暂 态 和 稳 态 阶段 均 优 于 PID 控制 器 。 与 自 适应 控制 器 相 比 ,该 控制 器 的 跟踪 误差 收敛 速 
度 快 于 自 适应 控制 方法 ,具有 和 较 好 的 暂 态 控制 性 能 。 这 是 因为 本 节 使 用 的 模型 降 维 算法 可 以 
减少 机 器 人 动态 模型 的 元 余 信息 ,从 而 获得 更 好 的 参数 估计 性 能 。 图 8. 52 为 七 个 关节 跟踪 误 
差 的 均 方 根 误差 (Root-Mean-Square Error, RMSE) 分 布 图 。 从 图 中 可 以 清楚 地 看 出 , 稳 态 误 
差 和 瞬 态 误差 均 较 前 两 者 有 较 大 的 改善 ,验证 了 该 控制 器 的 有 效 性 。 


0.6 
1.0 一 FT 2.0 FT 
oa os | ，-- 自 适 应 控制 --- 自 适应 控制 
VV ~ 一 了 ID 15 一 PID 
0.2 2 一 - 参考 轨迹 一 - 参考 轨迹 
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© 

跟踪 性 能 gq/rad 
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-0.4 
5 5 10 15 20 0 5 10 15 20 
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图 8.47 关节 3 跟踪 性 能 图 8.48 关节 4 跟踪 性 能 
— FT 
1.0 1.0 一 自 适应 控制 
—- PID 
县 05 了 os --- 参考 轨迹 
Sn Sn 
由 汪 
超 0 = 起 一 0 
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二 --- 自 适应 控制 洲 
—0.5 ws ES 有 
--- 参考 轨迹 | 
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时 间 /s 时 间 /s 
图 8.49 关节 5 跟踪 性 能 图 8.50 关节 6 跟踪 性 能 


跟踪 性 能 q/rad 


时 间 /s 时 间 /s ，. 
图 8.51 关节 7 跟踪 性 能 图 8.52 跟踪 误差 的 RMSE 


为 进一步 验证 参数 估计 性 能 ,引入 估计 参数 的 均 方 根 误差 为 ervse = 》) NV($, 一 8,)?/n， 
， 2 


其 中 ?表示 参数 的 数量 。 参 数 估计 性 能 如 图 8. 53 和 图 8. 54 所 示 。 从 图 8. 53 中 可 以 看 出 ,所 
有 估计 的 惯性 参数 都 可 以 在 很 短 的 时 间 内 收敛 到 其 真实 值 的 一 个 小 范围 内 ,而 图 8. 54 中 均 方 
根 误差 (RMSE) 最 终 也 收敛 到 零 附近 的 一 个 小 邻 域 。 
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图 8.53 基于 有 限时 间 算 法 估计 的 惯性 参数 估计 性 能 
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图 8.54 基于 有 限时 间 算 法 估计 的 惯性 参数 均 方 根 误差 
在 此 基础 上 ,对 本 节 介 绍 的 有 限时 间 估 计算 法 和 其 他 自 适应 估计 方法 的 参数 估计 性 能 进 
行 了 比较 。 具 体 估计 参数 如 表 8. 5 所 示 ,其 中 分 别 给 出 了 实际 值 、 本 节 的 有 限时 间 算 法 估计 的 
参数 以 及 其 他 自 适应 估计 方法 估计 的 参数 。 由 表 8. 5 可 以 看 出 ,采用 本 节 方 法 估计 的 参数 与 
实际 动态 参数 较为 接近 , 比 传统 的 自 适 应 估计 方法 具有 更 高 的 估计 精度 。 
表 8.5 动态 参数 的 估计 性 能 


由 认 有 限时 间 自 适应 方法 史记 证 有 限时 间 自 适应 方法 
算法 估计 值 估计 值 算法 估计 值 估计 值 
1 07 1322 0. 0864 0. 0484 
2 0. 0266 
3 —4. 1. 4161 
4 1. 5552 16 S79 
5 0. 9955 一 0.0279 
6 1. 5274 18 0. 4194 
7 0.0211 19 一 0.2221 
8 0. 1666 0. 0683 20 0. 3696 
9 0. 4057 0. 3986 0. 4159 0. 1070 
10 0. 0168 一 0.0214 
11 一 0.0241 0. 0084 
12 0. 0087 0.0334 0. 0360 0. 0105 
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算法 估计 值 估计 值 算法 估计 值 
县 卫生 
[oo | -oo0s | oo | s+ | -oo | -0.0008 | 
本 革 证 轩 记 7 站 宣 和 和 攻守 
ss 
a 


0. 0006 一 0. 0007 0.0146 | 43 | 0.0002 0. 0002 


注意 : 本 节 介 绍 的 是 基于 牛顿 - 欧 拉 模型 的 未 知 机 器 人 动力 学 参数 估计 问题 。 采 用 基于 
奇异 值 分 解 的 模型 降 维 算法 去 除 模 型 中 的 宛 余 信息 ,可 有 效 提 高 参数 估计 的 效率 和 精度 。 与 
其 他 估计 方法 相 比 ， 本 节 介绍 的 牛顿 - 欧 拉 模型 要 简单 得 多 ,更 适用 于 高 自由 度 机 器 人 的 实时 
估计 。 


8.4.5 相关 拓展 
1. 证 明 1 
考虑 以 下 李 雅 普 诺 夫 函 数 . 


续 表 
自 迁 度 方法 


一 55 (8. 147) 


对 式 了 二 D9 一 D9 二 D9, 在 P=0,T 宇 T, 以 及 D 满 足 IE 条 件 下 ,6 会 收敛 于 0。 
在 P 关 0 条 件 下 ， 147) 对 时 间 进 行 求 导 , 并 将 式 (8. 129) 代 入 得 到 1 FF 


2 ee 一 兴 
代入 自 适应 规律 5 一 一 r 下 | ,得 到 工 , 9 本 方 | por 注意 到 矩阵 是 正定 的 ， 
因此 D 的 所 有 奇异 值 都 大 于 零 , 考 虑 6 是 一 个 矢量 ,因此 可 以 推导 出 性 质 5 DO 宇 4 (D)0'0。 
: hi 区 D) . 
然后 有 工 , 一 Ti Fr A AI， 其 中 三 je om a 
为 IE 条 件 满足 ,可 以 根据 式 (8. 151) 推 导出 D>cT 和 定义 2, 始 终 存在 正 的 常数 /使 wx 之、 
人 有 
Ts uC- !) 。 根 据 式 (8. 147) 的 结果 ,可 以 得 到 6 在 有 限时 间 z, 志 2 VL C0)74 十 
T。 收敛 到 0。 
2. 证 明 2 
考虑 以 下 李 雅 普 诺 夫 孔 数 : 
ys =7eIMe, + FBT7G, (8. 148) 
对 工 v, 求 导 ,有 : 
ei + FelTMe, + Ir7@, (8. 149) 


将 式 (8.145) 代 入 式 (8. 149) 可 得 : 
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Ly 一 e7T( 一 Kie, 十 S, (0 6,q)G6 二 7,)— 
aC + Fe Me, + BT, (8. 150) 
将 自 适应 规律 式 (8. 146) 代 入 式 (8. 150) ,得 到 


< 到 $, — QB. 
Ly <er(—Kies +S,B, te) — SI (STe, 一 一 
i$,—0QB. | 
起 $, — QB. ele, 
ce (8. 151) 
TD 二 
-根据 式 (8. 139) ,考虑 FG ,= 人 一 0B, ,B= 二 U.G$, , 式 (8.151) 可 以 写成 如 下 形式 ， 
, A $, — QB. 有 
ba 1 
1$,—0QB. | El 
全 $, B. 5 
RM ee ee 
1$,—0QB. | | e, | 
$,— 0B. 
<— (6,—0B.) 一 一 一 一 一 十 | FG | 一 Ke 
1$,—0QB. | 

入 一 1 人 .一 0B8.|1 十 | FG,| —K,|e,| 

<— 1,—QB,+6$,—6,| 十 |FG, | 一 | e.l 

<— | FG, 一人. | 十 |FG6,. 1 —K,|e,| 

委 一 15,. | 一 Ke, ll 十 2 FG，I 


<— py aa 十 pa (8 152) 
其 中 ,py 一 min{ 2 M2/Amax (M)},04=21F | | @, 医 因为 下 和 天 ， 都 是 
有 界 的 , 且 lim 下 一 0, 存 在 一 个 0 附近 的 紧 集 0Q: 二 {8B,1L4 0G.) 委 (cu ) 使 得 VG-E&D， 


Ls 过 0,c 是 一 个 较 小 的 正 的 常数 ,满足 < 之 yuov ,0< ys<1。 根 据 前 述 公 式 可 以 推出 ,2 以 外 
的 信和 号 能 够 在 有 限 的 时 间 内 进入 这 个 紧 集 ,并 仍 将 在 未 来 一 直 保 持 。 因 此 ,参数 估计 误差 能 够 
在 有 限 的 时 间 内 收敛 到 真实 值 的 一 个 小 的 紧凑 集 。 此 外 ,可 以 通过 选择 适当 的 Pu 和 天， 来 选 
择 较 大 的 uy, 当 too 时 ,pa 收敛 于 0。 这 意味 着 t 足够 大 时 , 紧 集 Q 较 小 , 且 将 收敛 于 零 。 
可 以 通过 选择 一 个 更 大 的 Ps 增 大 收敛 速度 。 


8.5 神经 网 络 自 适应 导 纳 控制 


本 节 将 介绍 一 种 基于 机 器 人 -环境 交互 的 神经 网 络 自 适 应 导 纳 控制 方法 。 

随 着 机 器 人 技术 的 飞速 发 展 ,机 器 人 在 教育 .工业 、 娱 乐 等 领域 受到 了 广泛 的 关注 。 在 这 
些 领域 ,机 器 人 不 可 避免 地 与 外 部 环境 交互 。 虽 然 机 器 人 经 过 了 几 十 年 的 发 展 , 但 由 于 机 器 人 
在 更 一 般 的 场景 中 的 高 期 望 以 及 机 器 人 工作 的 复杂 环境 , 仍 有 许多 问题 坡 待 解决 。 为 了 实现 
机 器 人 的 柔性 行为 ,有 3 种 方法 得 到 了 广泛 的 应 用 : 导 纳 控制 ; 混合 位 置 / 力 控制 ; 阻抗 控制 。 

近年 来 ,由 Hogan 提出 的 阻抗 控制 概念 ,在 机 器 人 领域 得 到 了 广泛 的 应 用 。 阻 抗 控制 旨 
在 调节 机 器 人 在 物理 交互 点 上 的 行为 。 阻 抗 控制 的 核心 思想 是 控制 器 调节 机 械 阻 抗 , 即 广义 
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速度 到 广义 力 的 映射 。 另 一 种 方法 是 由 Mason 提出 的 导 纳 控制 。 在 一 个 广义 导 纳 控制 系统 
中 ,通过 测量 环境 力 和 所 需 的 导 纳 模型 ,得 到 了 一 个 参考 轨迹 并 进行 跟踪 。 然 后 ,通过 轨迹 适 
应 来 实现 柔性 行为 。 传 统 的 机 械 臂 控制 方法 大 多 是 基于 系统 模型 ,具有 良好 的 控制 性 能 。 然 
而 ,这 些 方法 需要 精确 的 机 器 人 模型 ,而 在 很 多 情况 下 精确 的 模型 无 法 获得 。 因 此 , 自 适应 控 
制 方法 受到 了 广泛 的 关注 ,并 在 实际 系统 中 得 到 了 应 用 。 采用 智能 工具 对 自 适 应 控制 的 不 确 
定性 进行 逼近 。 导 纳 控制 系统 的 另 一 个 关键 元 件 是 力 传感器 。 力 传感器 被 认为 是 机 器 人 与 环 
境 之 间 交 流 的 媒介 。 然 而 ,安装 在 机 械 臂 上 的 力 传感器 可 能 会 带 来 不 便 , 而 且 通 常 价格 昂贵 。 
由 于 这 些 原因 ,无 传感器 控制 方案 受到 了 广泛 的 关注 。 对 电机 的 外 力 估计 主要 有 两 种 方法 ， 
扰动 观测 器 法 和 基于 电机 扭矩 知识 的 力 观 测 器 法 。 如 基于 关节 角 的 干扰 观测 器 方法 和 基于 广 
义 动 量 的 碰撞 检测 力 观测 器 。 

基于 阻抗 / 导 纳 的 控制 ,阻抗 / 导 纳 模型 是 非常 重要 的 。 在 实际 应 用 中 ,由 于 环境 动力 学 的 
复杂 性 ,获得 理想 的 阻抗 / 导 纳 模型 并 不 容易 。 此 外 ,固定 阻抗 / 导 纳 模型 并 不 适用 于 所 有 人 情 
况 。 为 了 解决 这 一 问题 ,迭代 学 习 已 被 广泛 研究 ,用 于 机 器 人 适应 未 知 环境 。 该 方法 旨 在 通过 
重复 一 项 任务 将 人 类 的 学 习 技 能 引入 机 器 人 ,并 得 到 了 广泛 的 研究 。 如 采用 基于 神经 网 络 的 
方法 来 调节 机 器 人 末端 执行 器 的 阻抗 参数 。 然 而 ,这 种 学 习 方 法 的 缺点 是 需要 机 器 人 重复 操 
作 来 学 习 阻 抗 参数 ,这 在 实际 应 用 中 可 能 会 带 来 很 多 不 便 。 因 此 ,有 学 者 提出 了 阻抗 自 适应 方 
法 ,如 采用 在 不 同 阻 抗 参数 值 之 间 切 换 的 策略 ,分散 系 统 的 能 量 ; 应 用 于 机 器 人 与 未 知 环境 交 
互 的 阻抗 自 适 应 方法 等 。 

交互 控制 的 控制 目的 是 实现 力 的 调节 和 轨迹 跟踪 。 因 此 ,应 当 考 虑 到 最 优化 ,因为 它 是 这 
两 个 目标 的 综合 。 众 所 周知 的 线性 二 次 型 调节 器 (Linear Quadratic Regulator, LQR) 是 最 优 
控制 的 一 个 重要 解决 方案 , 它 涉及 以 最 小 的 代价 运行 一 个 动态 系统 。 例 如 ,在 已 知 环境 动力 学 
的 情况 下 ,用 LQR 法 求 理想 的 阻抗 参数 。 也 有 研究 人 员 根 据 环境 刚度 和 阻尼 对 所 定义 的 
LQR 问题 进行 在 线 求解 ,调整 目标 阻抗 。 该 算法 比 LQR 技术 获得 的 固定 阻抗 参数 具有 更 好 
的 适应 性 , 即 适用 范围 更 广 。 然 而 ,本 节 中 环境 的 动力 学 也 假定 是 已 知 的 。 因 为 在 环境 动力 学 
未 知 的 情况 下 ,Riccati 方 程 的 解 是 很 难 找到 的 。 因 此 , 当 环 境 动力 学 未 知 时 ,上 述 方法 可 能 不 
适用 。 为 了 解决 这 一 问题 , 自 适 应 动态 规划 (Adaptive Dynamic Programming,ADP) 得 到 了 
广泛 的 关注 和 研究 ,尤其 是 用 ADP 解决 优化 问题 。 基 于 ADP 的 思想 可 以 利用 环境 反馈 信息 
对 控制 行为 进行 修正 。ADP 方法 有 许多 ,如 启发 式 动态 规划 和 Q 学 习 , 其 优点 是 只 需要 知道 
被 控制 系统 的 部 分 信息 。 

本 节 介 绍 了 一 种 导 纳 自 适应 方法 来 调整 导 纳 参数 ,实现 未 知 环境 动态 下 的 最 优 交 互 行为 。 
考虑 具有 未 知 动力 学 的 环境 ,将 其 建 模 为 状态 空间 形式 的 线性 系统 。 控 制 目标 是 使 所 定义 的 - 
成 本 函数 最 小 化 ,使 其 具有 良好 的 交互 性 能 。 采 用 基于 广义 动量 法 的 观测 器 来 估计 系统 的 相 
互 作用 力矩 ,同时 采用 基于 神经 网 络 的 控制 器 来 保证 系统 的 跟踪 性 能 。 


8.5.1 问题 描述 
1. 系统 模型 描述 
系统 中 ,考虑 机 械 臂 与 环境 的 交互 作用 , 正 运动 学 可 以 表示 为 : 
ME 十 Ce 十 GEg 一 一 ren 
《8 1537 
CFd 十 CGFd9 et 
式 (8.153) 分 别 给 出 了 线性 空间 下 ,用 质量 -阻尼 -刚度 模型 和 阻尼 -刚度 模型 表示 的 动态 模型 。 
其 中 ,ME Cr 和 GE 分 别 表示 模型 中 未 知 的 质量 、 阻 尼 和 刚度 和 矩阵。 上 述 两 种 模型 可 以 用 来 
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表示 大 多 数 环境 动力 学 。 

2. 代价 函数 

控制 的 目标 是 通过 设计 的 控制 系统 使 得 代价 函数 最 小 。 考 虑 机 器 人 系统 状态 以 及 外 部 环 
境 ,设计 代价 函数 如 下 : 


Vz) = Ug) Og) He Re dt (8. 154) 


其 中 ,0 二 0 ER”"*” 是 正定 矩阵 ,表示 跟踪 误差 权重 ,08 ER"*" 表示 交互 力 权重 和 矩阵 。 代 价 
函数 用 来 评价 交互 控制 效果 ,通过 最 小 化 V(1) ,可 以 获得 期 望 的 交互 性 能 。 
8.5.2 基于 神经 网 络 的 自 适 应 控制 器 设计 
1. 自 适 应 最 优 控制 计算 
考虑 如 下 连续 时 间 的 线性 系统 
é£=Aé& Butlt) (8. 155) 
其 中 ,EER” 是 系统 状态 变量 ,wu ER 为 系统 输入 ,4ER"x”" 是 系统 矩阵 ,输入 矩阵 BE 
有 "> , 且 4.B 都 是 未 知 的 常数 矩阵 。 
系统 最 优 控制 输入 为 : 
=—Ké (8.156) 
它 可 以 最 小 化 代价 函数 : 


v=| (erQé +uTRu)d (8. 157) 


式 (8.157) 的 解 与 LQR 问题 的 解 类 似 。 根 据 最 优 控制 理论 ,可 以 在 已 知 矩 阵 4 和 B 的 基 
础 上 ,通过 解 ARE 方程 得 到 和 矩阵 也 ”: 
P4 十 4IP 十 OO 一 PBR-BIP 一 0 (8. 158) 
然后 ,由 已” 可 得 最 优 反馈 增益 如 下 : 
K™ =—R "BPp” (8. 159) 
因此 ,可 以 得 到 式 (8. 159) 的 最 优 控制 输入 。 接 下 来 ,使 用 在 线 学 习 算 法 来 获得 在 环境 动力 学 
未 知情 况 下 的 最 优 控制 输入 。 


定义 P,ER”xm 将 矩阵 转换 为 向 量 形式 , 即 : P, ER”*" 一 记 , ER'”"1?, 同 理 ,EE 


R"~EER ,其 中 P, 是 对 称 矩阵 : 
: = .PP 3 


mm 


E =LéT E12 EE rE E23 es 1 
Sse=[e(h 7 — Eto Flt) — EG EC — Ea] 


> S | (8.160) 
re=||"®e,)e@e,,) #@d 

1 = ||"s@u, | Gu, Bu 
其 中 ,加 表示 克 罗 内 克 乘 积 ,! 为 积分 次 数 。 假 定 初始 输入 二 Ko 十 $,tE [to ,ti],$ 是 噪声 ， 
K。 表示 初始 反馈 增益 , 它 的 选取 要 能 够 保证 系统 的 稳定 。 然 后 ,计算 Ts、16 直到 满足 如 下 
矩阵 的 秩 关系 : 


_m(m+t1) 


rank( Ll La)) 5 


十 mr (8.161) 
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满足 式 (8. 161) 后 ,可 由 如 下 齐 次 方程 求 得 PK 、 氏 K+l: 


| 和 | -efeo-e iv 8.16 约 
Vec (天 K+H1) 
相关 变量 定义 如 下 : 

@, =[6¢6e, — 2Iee 1,, 因 开 起 ) 一 2T (1, © R)] 

EC =— Teevec (Q,) (8. 163) 


Qu =Q + KARK, 

其 中 ,1,, 是 m 维 单位 阵 ,vec(，) 是 将 矩阵 转换 为 向 量 的 函数 。 重 复 计 算 , 直到 ‖ Pr 一 
Prk_i | <s, 其 中 的 e 为 很 小 的 正 实 数 。 由 式 (8. 162) 可 得 最 优 反馈 控制 增益 Kk 。 

2. 自 适应 导 纳 控制 

这 部 分 的 目标 是 获得 一 个 在 未 知 环境 的 最 优 导 纳 控制 模型 ,使 代价 函数 最 小 。 这 里 使 用 
的 是 环境 模型 阻尼 -刚度 模型 : Crd 十 Grdq 王 一 rs。 定义 如 下 状态 变量 : 

6E[ 0 (8. 164) 

则 式 (8. 154) 可 写成 如 下 形式 : 


V -| (te OQ | | HET Re ) dt 
0 da 


-| (£TQ'E HET RE) dt (8. 165) 
其 中 : 
太守 0 Bd 
- 0 
结合 所 定义 的 状态 变量 ,可 以 将 环境 模型 重 写 为 状态 空间 形式 : 
£=AE+ BE (8. 166) 
其 中 : 
EY oe! _r-l 
-| CE ce 3 | | (8. 167) 
0 i 0 


由 式 (8. 167) 不 难看 出 ,矩阵 A 和 B 包含 未 知 的 环境 动态 。 将 fs 作为 系统 输入 ,用 上 述 介绍 
的 自 适 应 最 优 控制 的 方法 得 到 系统 最 优 的 反馈 控制 信号 。 
显然 ,矩阵 4 和 B 包含 了 未 知 的 环境 动力 学 。 如 果 将 fss 作为 式 (8. 166) 的 输入 ,可 以 使 
用 上 述 方法 得 到 系统 的 控制 输入 ,以 使 代价 函数 最 小 化 : | 
f=—Krké (8. 168) 
其 中 ,Kk 由 前 述 自 适 应 在 线 学 习 的 方法 得 到 。 
从 LQR 角度 理解 式 (8. 168) , 则 最 优 控制 输入 由 式 (8. 159) 求 得 。 求 解 ARE 方程 ,可 得 


最 优 和 矩阵 已” 为 
By -E> 
Pp = (8. 169) 
x 关 
其 中 ,PER”*",P, ER”"™”。 
因此 ,可 得 . 
t= RR pia—R Pyg (8. 170) 


将 式 (8. 170) 与 期 望 导 纳 模型 进行 比较 ,可 得 期 望 导 纳 模型 ,并 保证 最 优 交 互 性 能 。 参 考 
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轨迹 g。， 和 环境 外 部 力矩 的 估计 值 站 由 力 观测 器 估计 , 则 可 得 关节 空间 的 修正 参考 轨迹 g,。 
3. 控制 器 设计 
系统 的 内 环 是 为 了 保证 机 械 臂 的 轨迹 能 够 跟踪 参考 轨迹 g, 。 为 此 ,设计 了 一 种 基于 自 适 
应 神经 网 络 的 控制 器 。 考 虑 机 械 辟 的 动力 学 特性 ,定义 跟踪 误差 如 下 : 
i 
(C8, 171 
| 一 diag(A1 A," A, ) 
其 中 ,es 一 q 一 q, ,4;0。 
引用 nn 连 杆 的 机 器 人 的 动力 学 公式 : 


| :M(g)g+C(g,9)9+G(g) 十 ro 一 = (8. 172) 
将 式 (8.171) 代 入 式 (8. 172) 得 : 
M(g);++C(l(g,9)s+G(gq)++ M(gqNvV tC(g,9)v =r Oo To (8. 173) 
控制 器 的 输入 力矩 为 : 
r 一 和 十 Mi 十 Co 十 fs 一 Ks (8. 174) 


入 入 J > i 1 六 
其 中 ,M(g).C(g,9) 和 G' (9) 是 神经 网 络 的 估计 项 , 开 王 diag(Ri ,2 Ri Ri 全 本 ,闭环 系统 
可 写 为 ， 
M(o) +CCg0)s + Ks=— Mg) —M(g)y — (Cg,9) 一 Coa))u 一 


(G(g) —G6(g)) 一 (人 一 ro) (8. 175) 
用 RBFNN 通 近 法 可 得 : 
=WhZm (gq) +em(g) 
Cl(g,9) =WLZc(g,0) ec(g) (8. 176) 


G(g) =W6LZ6(g) +ec(g) 
其 中 ,Wm We 和 Woe 是 理想 的 权重 和 矩阵。 沁 斌 (gq) ,Ze (gd) 和 Zc (是 RBFNN 的 基 范 数 ， 
定义 如 下 : 
ZMm(q)=diag(Z, ,2Z,) 
{Zc(g,9)=diag([2,,2,] ,LZ,,2,]) (8.177) 
Zo ta di 3 


等 


gt dp | 


(8. 178) 
2; 一 [go 人 | 一 0 | en sp | 9 = lL >] 
式 (8.178) 中 ,p(。) 表 示 高 斯 函数 。 
M(g).C(g,9) 和 G(g) 的 估计 表示 为 : 

一 邦和 Zr (gq) 
Co,o) =WLZc (gqg,0) (8. 179) 
Gg) =WLZo (gq) 

将 式 (8. 179) 和 式 (8.177) 代 入 式 (8.175) 中 ,可 得 .: 

M(g)s+C(g,90)s + Ks =— WhiZuy — WiZev — WELZo—e, (8. 180) 


150 二 | 机 器 人 控制 一 一 运动 学 、 控 制 器 设计 、 人 机 交互 与 应 用 实例 


其 中 ,Wu =Wm Wm ,We =We=—We ,Wo =Wse 一 全。 Ce 


4. 稳定 性 分 析 
选取 李 雅 普 诺 夫 函 数 如 下 : 
V=s s Ms 十 了 trCWEOwWw) 十 二 FWQcWe 站 Fr WiQ6 Wo) (8.181) 


其 中 ,Om \@c- 和 QQ 是 正常 数 矩 阵 。 
对 李 雅 普 诺 夫 函 数 即 式 (8. 181) 求 导 , 可 得 : 


六 下 Fs Ms +tr WHO Wa) + tr WIQWe) +tr WiQOWe) (8.182) 


理想 的 权重 矩阵 Ww .We 和 Wo 是 常数 矩阵 ,有 如 下 关系 : 


W. = (8. 183) 
Wo = W, 


由 于 2C(g， 4) 一 M(q) 是 反对 称 矩 阵 ， 将 式 (8. 182) 变 为 ， 
V=s'Mi+s'Cs+ tr(WHOuv Wu) +trWIQWe) + tr WIQeWe) 
=—s' (Ks HWUZuo HWLZcv +HMLZG + e, + 
tr (WIOuWM) + tr WIQOcWe) + tr WIQeWe) 
=—s'Ks—s'e, — trWh (ZuvsT + QuWu)] 一 
tr[WE ZevsT + QeWe)] — tr[WE (ZcsT + QoWo)] (8. 184) 
设计 RBFNN 的 更 新 律 : 
| =—QM (Zuvds' +ouWu) 


We =—Qa (ZevsT +ocWe) (8. 185) 


W, =—Qa (Zo6sT +ocW,) 
将 式 (8.184) 代 入 式 (8. 183) 中 : 
V=—s'Ks—s!e, + touWaWu] + tlocWEWe] + tLeoWeWo] (8. 186) 
根据 et 
T 2 十 2 OM | 这 2 2 56c | 个 2 
Ve Ks 十 二 上 si 十 二 11es 1 +a 一 了 Wuwl = 一 六 We 
C8. 7 
其 中 ， 


IGM oc OG ; 
“一 了 | Wa 上 一 本 5 | ws | 


当 式 (8. 187) 满 足 : 
1 


fies E 2 2 二 "1 古 -12 十 21 六 1 
a 雪 红 ( 玫 一 于 中: 十 却 | ec 用 十 过 机 和 十 肥 1 郊 c 有 十 过 让 We 1 
(8. 188) 


其 中 ,和 矩阵 工 是 单位 矩阵 。 
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利用 UUB 稳定 分 析 : 跟踪 误差 ; ,外 力 估计 误差 e. ,神经 网 络 误差 斤 ，. 钱 。 和 于 。 将 收 
敛 至 一 个 不 变 集合 (2,),2, 可 以 定义 为 : 


Q,=ICWu ll, wel, wel, le,l,ls |), 


这 Ww + 于 1We ?二 下 We 1 二 + < 
Q Qa Qa Q 

(8. 189) 
图 8. 55 给 出 了 此 不 变 集合 的 图 示 。 此 不 变 集 处 于 第 一 项 象限 ,其 中 集合 与 x 轴 、y 轴 、= 


轴 的 交点 为 gp.8, ( oe 1 2=0, Ns 1*=0), (s7 (Kk—t1)s =e, 


Te 2 
2 外 一 3 | 


Rs | ae 
Ww.., 有 一 0, el?=0), (Flerl? =a, |W.., 12=o,ls12=0). 


因此 ,图 8.55 中 ,定义 : 


——|W..,| =a, W=6 

2 

4s (K —F1)s =, = (8. 190) 
Flerl? =e, er: = 


8.5.3 仿真 控制 

这 里 对 一 个 两 连 杆 机 械 臂 与 未 知 环境 的 物理 交互 进行 仿真 。 仿 真 环 境 如 图 8. 56 所 示 , 环 
境 的 外 力 仅 作用 于 机 械 臂 的 Y 轴 方 向 。 两 连 杆 机 械 臂 参数 设置 为 : m1] 二 2kg,m, 二 2kg,1i 一 
0.2m,1, 二 0.2m, 了 1 二 0.027kgm*, 了 ,二 0.027kgm?,g 二 9. 81m/s*。 控 制 器 的 控制 目标 是 保 
证 机 械 臂 能 够 很 好 地 跟踪 参考 轨迹 q，, 。 


一 一 参考 轨迹 


pb xe 
2 ¥ 
lled 
站 
le ; 
图 8.55 不 变 集 合 Q, 图 示 图 8.56 系统 仿真 场景 
1. 交互 性 能 


假定 环境 的 动态 模型 为 : 0.019 十 (g 一 0;3)= 二 一 tw ;采用 LQR 法 对 其 进行 验证 。 当 和 矩 
阵 4 和 B 已 知 时 ,用 ARE 方程 的 解 ,可 以 得 到 最 优 的 导 纳 模型 参数 。 

首先 ,需要 设置 合适 的 系统 变量 初始 值 。 噪 声 函 数 的 选取 ,关系 到 估计 的 参数 是 否 能 够 收 
敛 到 真实 值 。 选 取 噪 声 函 数 为 : 
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8 
a (8. 191) 
w=1 


为 保证 系统 的 稳定 性 ,选取 初始 的 增益 KK, 二 [一 1; 0. 1],Pk 的 初始 值 设 为 Pu, 王 107，。 
然后 , 设 定 最 优 的 控制 增益 Kk ,直到 满足 条 件 : ‖ Pk i 三 0. 02。 代 价 函 数 中 的 权重 设 定 为 
三 TR 三 

仿真 结果 如 图 8. 57 一 图 8. 60 所 示 。 当 矩阵 A 和 B 已 知 时 ,基于 LQR 方法 求 得 的 最 优 
导 纳 模型 为 : fs 王 一 0.41420 十 0.0702g, 。 如 图 8. 57 所 示 为 基于 LQR 法 和 自 适 应 在 线 学 
习 方 法 产生 的 关节 空间 中 机 械 臂 的 期 望 轨迹 和 参考 轨迹 。 可 以 看 到 ,在 初始 时 刻 ,基于 上 述 两 
种 方法 产生 的 轨迹 曲线 之 间 有 很 大 的 误差 ,主要 是 因为 初始 的 阻抗 模型 0. 019 十 (gq 一 0. 3)= 
一 rox 和 选取 的 噪声 函数 。 之 后 ,两 轨迹 曲线 之 间 的 误差 逐渐 减 小 ,最 终 两 轨迹 曲线 的 走向 趋 
近 相 同 。 


f 一 一 0.41426 十 0. 0702g, 


时 间 /s 
:i 一 9 
a -- LQR 
-~-- 自 适应 控制 器 
和 10 1$ 20 
时 间 /s 


图 8.57 期 望 轨迹 gs 和 两 种 方法 产生 的 修正 参考 轨迹 
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图 8.58 Q=1 和 R=1 时 ,Pk 和 Kxk 收敛 到 它们 的 最 优 值 


图 8. 57 表示 了 LQR 和 自 适应 最 优化 方法 产生 的 修正 参考 轨迹 曲线 。 
图 8. 58 给 出 了 上 述 两 种 方法 之 间 的 导 纳 参数 误差 ‖ K, 一 开 | ,说 明基 于 LQR 方法 的 
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交互 力 /Nm 
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图 8.59 Q=1 和 R=1 时 ,两 种 方法 下 交互 力 大 小 对 比 
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让 时 间 /s 
Ee 一 自 适 应 控制 器 
站 
0%0 5 10 15 20 
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8.60 Q==1 和 R=1 时 ,两 种 方法 下 代价 函数 趋势 


Pk 和 Kk 最 优 解 是 收敛 的 。Pk 和 Kk 的 差 值 在 12 次 迭代 后 趋 于 0.01,Pk 的 欧 几 里 得 范 
数 误 差 减少 到 0. 02, 其 中 . 
0.0033 0.0027 0.0041 一 0.0007 
由 0027 0. | l 于 宅 0.0007 0. a 
最 终 ,由 在 线 学 习 的 方式 得 到 的 最 优 导 纳 模型 为 ， 
人 一 一 0.47326 十 0.09049 ， 
对 比 由 LQR 产生 的 最 优 导 纳 模型 ,可 知 本 节 所 介绍 的 算法 是 有 效 的 。 

为 了 进一步 验证 该 方法 的 正确 性 ,给 出 了 当 Q=5 和 尺 ==1 时 ,代价 函数 有 不 同 权 重 , 结 果 
如 图 8. 59 一 图 8. 62 所 示 。 同 样 地 ,期 望 的 导 纳 模型 ?二 一 1. 44959 十 0. 2033gq, 。 基 于 自 
适应 在 线 学 习 方 法 ,迭代 三 步 后 可 得 导 纳 模型 ?二 一 1. 53749 十 0. 2063g, 。 当 Q=5 和 
R= 二 1 时 ,期 望 轨迹 g， 和 参考 轨迹 g, 如 图 8. 59 所 示 。 如 图 8. 60 所 示 , 经 过 12 次 迭代 ,LQR 
与 自 适应 在 线 方法 之 间 的 误差 | K, 一 多 “收敛 到 0.08。 如 图 8. 61 所 示 ,在 7s 时 ,相互 作用 
力 收敛 到 0. 35nm 左右 。 两 种 方法 对 应 的 代价 函数 值 分 别 为 0. 561 和 0. 602。 同 样 地 , 当 
Q==5,R= 二 1 时 ,基于 上 述 两 种 方法 所 得 的 对 称 正和 矩阵 Pk 和 P” 为: 

_ [0.0106 0.0068 ,| ,00L45 =—O,.0020 
" 上 0068 a ; 加 区 0.0020 0. | 
将 自 适应 在 线 学 习 方 法 所 得 导 纳 模型 与 LQR 方法 所 得 导 纳 模型 进行 比较 ,可 以 发 现 两 


K 


K 
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者 的 差异 很 小 。 因 此 ,本 节 所 述 方法 的 总 体 效果 不 错 。 
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图 8.61 Q==5 和 R=1 时 的 期 望 轨迹 g, 和 参考 轨迹 
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图 8.62 Q==5 和 RR=1 时 ,Pk 和 Kk 收敛 到 它们 的 最 优 值 
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图 8.63 Q==5 和 尺 ==1 时 ,两 种 方法 下 交互 力 大 小 对 比 
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图 8.64 Q=5 和 RR=1 时 ,两 种 方法 下 代价 函数 趋势 


2. 自 适应 神经 网 络 控制 器 的 仿真 验证 
两 连 杆 机 械 臂 的 关节 角 初 始 值 设 为 , g(0) 一 [0. 26, 一 0. 26]7 ,输入 参考 轨迹 为 : gy 一 


[0.3 十 0. 2e 一 ,0. 3e-']7 ,仿真 时 间 z. = 二 20s。 设 权重 和 矩阵 的 初始 值 为 :， WY (0)= 二 0E€ RX/， 


WL(0)= 二 0ER”*',W6,(0)= 二 0ER”*!。 跟 踪 性 能 结果 如 图 8. 65 一 图 8.71 所 示 。 图 8. 65 中 
机 械 臂 的 实际 轨迹 可 以 很 好 地 跟踪 参考 轨迹 , 且 从 图 8. 66 中 可 以 看 到 ,跟踪 误差 最 终 收敛 并 
趋 近 于 0。 图 8.67 一 图 8. 69 所 示 为 自 适 应 在 线 学 习 神 经 网 络 的 函数 逼近 性 能 。 从 这 些 图 中 
可 以 看 出 ,本 节 介 绍 的 算法 可 以 跟随 非 线 性 函数 的 动力 学 (M(g),C(g,9),G(gq,9)), 并 取得 
满意 的 跟踪 性 能 。 如 图 8. 70 所 示 权 重 和 矩阵 有 收敛 的 趋势 ,图 8.71 为 控制 锅 的 输入 r; 和 +,， 
说 明 输 入 控制 力矩 信号 是 稳定 且 有 界 的 ,其 中 神经 网 络 的 参数 随 着 时 间 的 推移 变 得 逐渐 稳定 。 
实验 的 结果 说 明 自 适应 神经 网 络 控制 器 取得 了 令 人 满意 的 控制 效果 。 
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图 8.65 每 个 关节 控制 器 的 跟踪 性 能 


3。 力 观测 器 

假设 交互 力作 用 于 机 械 臂 的 端 部 ,观测 器 对 外 部 力 的 估计 结果 如 图 8.72 和 图 8. 73 所 示 。 
图 8.72 给 出 了 交互 力 的 实际 值 及 其 估计 值 ,各 关节 广义 动量 的 估计 结果 如 图 8. 73 所 示 。 从 
图 中 可 以 看 出 ,观测 器 输出 的 交互 力 和 关节 广义 动力 与 实际 值 一 致 , 表 明 该 观测 器 具有 较 好 的 
估计 性 能 。 
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图 8.66 每 个 关节 控制 器 的 跟踪 误差 
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图 8.67 基于 RBFNN 的 M(o) 和 矩阵 的 函数 逼 近 能 力 
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图 8.68 基于 RBFNN 的 C(o,g) 矩 阵 的 函数 逼近 能 力 
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图 8.69 基于 RBFNN 的 G(o) 和 矩阵 的 函数 逼近 能 力 
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图 8.70 每 个 关节 NN 控制 器 权 值 更 新 
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图 8.71 控制 器 的 输入 r; 和 r， 


交互 力 /Nm 


0 二 10 15 20 
时 间 /s 


图 8.72 ”交互 力 的 真实 值 和 估计 值 
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图 8.73 从 力矩 观测 器 得 到 的 各 关节 广义 动量 的 估计 值 和 真实 值 


8.6 本 章 小 结 


本 章 首先 介绍 了 自 适应 控制 的 基本 原理 ,然后 基于 这 一 原理 ,结合 不 同 的 机 器 人 应 用 场 
景 ,详细 阐述 了 自 适应 控制 器 的 设计 。 感 兴趣 的 读者 可 以 根据 自己 目前 的 研究 方向 进一步 深 
和 研究 。 


RN 


其 他 控制 方法 


CHAPTER 9 


除了 第 7 章 和 第 8 章 中 介绍 的 神经 网 络 控制 和 自 适应 控制 ,还 有 很 多 其 他 的 智能 控制 方 
法 ,如 滑 模 控制 .学 习 控 制 、 模 糊 控制 以 及 这 些 方法 相 结合 ,或 者 这 些 方法 和 神经 网 络 相 结合 形 
成 的 新 的 控制 方法 。 本 章 将 结合 具体 应 用 来 研究 基于 上 述 控制 算法 的 控制 器 设计 。 


9.1 其 他 控制 方法 介绍 


本 节 将 介绍 滑 模 控制 .学 习 控 制 .模糊 控制 等 控制 方法 ,以 及 常 与 这 些 控制 方法 结合 使 用 
的 一 些 神经 网 络 , 如 径 向 基 函 数 (Radial Basis Function，RBF) 神 经 网 络 。 


9.1.1 滑 模 控制 

滑 模 控制 也 叫 变 结构 控制 ,本 质 上 是 一 种 非 线 性 控制 , 且 非 线性 表现 为 控制 的 不 连续 性 。 
变 结 构 控 制 起 源 于 继电器 控制 和 bang-bang 控制 ,与 常规 控制 不 同 , 该 方法 在 控制 上 不 连续 ， 
对 非 线 性 系统 具有 良好 的 控制 性 和 和 鲁 棱 性 。 滑 模 控 制 对 被 控 对 象 的 建 模 误差 对象 参数 以 及 
外 部 干扰 极 不 敏感 ,是 一 种 实践 性 很 强 的 控制 方法 。 从 19 世纪 60 年 代 苏 联 学 者 提出 发 展 50 
余年 至 今 , 它 已 在 电机 、 电 力 系统 、 机 器 人 、 圾 天 所 积 何 服 系统 中 得 到 f 泛 的 应 用 。 

1. 工作 原理 

考虑 一 个 非 线 性 系统 

= ER oe BE 《9.1) 
确定 切换 函数 向 量 *(z),sE 及 ” ,控制 器 设计 框架 


sa os) S30 
= (C9. 2) 
i s(xw)S0 


其 中 ,s (x) 为 切换 函数 , 当 切 换 函 数 取 值 不 同时 ,对 应 控制 器 的 结构 不 同 ,从 而 相 平 面 中 系统 
的 运动 轨迹 也 不 同 , 如 图 9. 1 所 示 。 在 合适 的 控制 规律 作用 下 ,系统 相 平 面 中 的 状态 (ZX ,zx ) 最 
后 运动 到 直线 中 ,这 种 运动 就 是 滑 模 运动 ,该 直线 也 叫 切换 线 , 可 由 用 户 自行 定义 ; 运动 到 切 
换 线 后 ,系统 状态 沿 着 直线 趋向 于 原点 ,从 而 保持 系统 的 稳定 。 由 上 上 述 分 析 可 知 * 清 本 控制 下 
的 系统 运动 状态 包括 两 个 过 程 , 即 趋 近 运 动 过 程 ( 从 系统 的 初始 状态 zu 运动 到 滑 模 面 状态 
A) 和 滑 模 运 动 过 程 ( 在 滑 模 面 上 从 状态 A 运动 到 原点 O) ,如 图 9.2 所 示 。 

2. 趋 近 律 

为 了 使 滑 模 面 以 外 的 系统 状态 能 够 快速 地 到 达 滑 模 面 并 完成 趋 近 运 动 ,需要 选取 一 个 好 
的 趋 近 律 。 因 为 过 大 的 趋 近 速 度 会 导致 系统 剧烈 拌 振 , 太 小 的 趋 近 速 度 则 会 使 系统 反应 过 慢 ， 
以 下 列举 几 种 常用 的 趋 近 律 。 


160 大 || 机 器 人 控制 一 一 运动 学 、 控 制 器 设计 、 人 机 交互 与 应 用 实例 


% Xo 
5 
和 s(x)=0 
图 9.1 滑 模 运动 示意 图 图 9.2 滑 模 运动 示意 图 
等 速 趋 近 律 : 
一 一 ssSgn(S) 之 0 (9. 3) 
指数 趋 近 律 : 
$=—éesgn(s)—ks ee —>0,k>0 (9.4) 
老 次 趋 近 律 : 
| 《92 5 
三 一 ssgn(5) 一 了 (ss) 二 0 (9.6) 
其 中 ,s 为 滑 模 算 子 ,表达 式 为 : 5 二 6 十 Ae。 
3. 震颤 问题 


滑 模 控制 方法 刚 提出 时 ,控制 器 输出 的 震 闸 问 题 就 已 经 成 为 滑 模 控 制 需 要 改进 和 解决 的 
问题 之 一 。 控 制 器 输出 的 震颤 将 会 导致 一 系列 的 问题 ,震颤 加 速 了 执行 器 的 机 械 磨 损 , 缩 短 系 
统 使 用 寿命 ,来 回 的 震荡 也 存在 共振 问题 。 其 发 生 的 原因 主要 有 以 下 几 点 。 

(1) 由 于 切换 函数 的 存在 ,系统 状态 在 滑 模 面 上 来 回 震荡 ,使 得 系统 维持 在 滑 模 面 上 并 不 
断 趋 近 于 零 ,在 切换 控制 器 结构 的 过 程 中 ,控制 器 输出 量 也 随 之 来 回 震荡 ; 

(2) 时 间 滞 后 开关 (控制 作用 对 状态 准确 变化 有 滞后 ); 

(3) 空间 滞后 开关 (状态 空间 中 的 状态 量变 化 死 区 ); 

(4) 系统 惯性 的 影响 ; 

(5) 离散 时 间 系 统 本 身 造 成 的 抖 振 。 

解决 震颤 问题 的 方法 目前 主要 有 以 下 几 种 : 

(1) 准 滑动 模 态 方法 (系统 运动 轨迹 被 限制 在 边界 层 ) ; 

(2) 趋 近 律 方法 (保证 动态 品质 减弱 控制 信号 拌 振 ) ; 

(3) 观测 器 方法 (补偿 不 确定 项 和 外 界 干扰 ) 。 


9.1.2 模糊 控制 


模糊 控制 是 一 类 应 用 模糊 集合 理论 和 模糊 逻辑 推理 的 控制 方法 。 一 方面 ,模糊 控制 提出 
一 种 用 于 实现 基于 知识 (规则 ) 甚 至 语义 描述 的 控制 规律 的 新 机 制 ; 另 一 方面 ,模糊 控制 为 非 
线性 控制 器 提出 一 个 相对 容易 的 设计 方法 ,尤其 是 当 被 控 装 置 ( 被 控 对 象 或 被 控 过 程 ) 具 有 不 
确定 性 而 且 很 难 用 常规 非 线 性 控制 理论 处 理 时 ,更 为 有 效 。 

模糊 控制 是 智能 控制 中 一 个 活跃 的 研究 与 应 用 领域 。 模 糊 控 制 系统 对 于 处 理 测量 数据 不 
确切 .数据 量 过 大 以 致 无 法 实现 实时 性 和 复杂 时 变 的 被 控 对 象 是 非常 有 价值 的 ,利用 模糊 系统 
来 盘 近 上 述 非 线性 系统 非常 有 效 。 与 依据 系统 行为 参数 的 传统 控制 器 设计 方法 不 同 , 模 糊 控 
制 器 设计 依赖 于 设计 者 的 经 验 。 在 传统 控制 器 中 ,其 核心 是 系统 的 数学 模型 ;而 模糊 控制 器 
控制 的 核心 是 模糊 规则 。 改 善 模 糊 控 制 性 能 的 方法 是 优化 模糊 控制 规则 、 调 整 模 糊 描 述 。 通 
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常 ,模糊 控制 规则 的 获取 是 通过 将 人 的 手动 控制 经 验 转化 为 模糊 语言 形式 ,因此 它 有 一 定 的 主 
观 性 ,而 且 有 限 的 规则 难以 准确 、 完 整地 描述 人 的 手动 控制 经 验 。 所 以 ,模糊 控制 也 存在 一 定 
的 局 限 性 和 不 足 ,没有 一 种 特定 的 控制 方法 对 所 有 的 控制 对 象 和 在 各 种 环境 下 都 是 最 优 的 。 

1. 模糊 控制 的 概念 和 特点 

模糊 控制 是 模糊 理论 在 控制 工程 上 的 应 用 。 它 用 语言 变量 代替 数学 变量 (或 将 两 者 结合 
应 用 ) ,用 模糊 条 件 语 句 来 描述 变量 间 的 函数 关系 ,用 模糊 推理 来 “刻画 ”复杂 的 关系 ,是 具有 模 
拟人 类 学 习 和 自 适应 能 力 的 控制 系统 。 模 糊 控 制 的 核心 是 模糊 规则 和 各 种 变量 的 模糊 集合 表 
示 。 一 个 典型 的 模糊 控制 系统 结构 示意 图 如 图 9. 3 所 示 。 


图 9.3 模糊 控制 系统 结构 示意 图 


模糊 控制 在 复杂 的 工业 生产 控制 领域 得 到 了 广泛 的 成 功 应 用 ,特别 是 在 近 二 十 年 来 发 展 
相当 迅速 ,这 主要 归结 于 模糊 控制 的 如 下 显著 特点 : 

。 无 须 建立 被 控 对 象 的 数学 模型 ; 

。 是 一 种 模拟 人 类 知识 .思维 智慧 的 控制 方法 ; 

。 规则 和 推理 机 制 易 被 人 接受 .理解 ,便于 进行 人 机 交互 ; 

。 控制 器 构造 容易 ; 

。 控制 器 鲁 棒 性 好 。 

2. 模糊 控制 器 的 结构 

模糊 控制 器 的 基本 结构 通常 由 4 个 部 分 组 成 : 模糊 化 接口 .规则 库 ,模糊 逻辑 推理 和 清晰 
化 接口 ,如 图 9.4 所 示 。 

1) 模糊 化 接口 

模糊 化 就 是 通过 在 控制 器 的 输入 、 输 出 论 域 
上 定义 语言 变量 ,将 精确 的 输入 、 输 出 值 转换 为 模 
糊 的 语言 值 。 模 糊 化 接口 的 设计 步骤 事实 上 就 是 
定义 语言 变量 的 过 程 ,可 分 为 以 下 几 步 。 

[语言 变量 的 确定 。 针 对 模糊 控制 器 每 个 输 
入 ,输出 空间 ,各 自 定义 一 个 语言 变量 。 通 常 取 系统 的 误差 值 。 和 误差 变化 率 ec 为 模糊 控制 
器 的 两 个 输入 ,在 e 的 论 域 上 定义 语言 变量 “误差 EF”, 在 ec 的 论 域 上 定义 语言 变量 “误差 变化 
EC”; 在 控制 量 的 论 域 上 定义 语言 变量 “控制 量 U”。 

@@ 语言 变量 论 域 的 设计 。 在 模糊 控制 器 的 设计 中 ,通常 就 把 语言 变量 的 论 域 定义 为 有 限 
整数 的 离散 论 域 。 例 如 ,可 以 将 五 的 论 域 定义 为 {一 m ,一 m 十 1,…, 一 1,0,1,…,m 一 1,m); 
将 EC 的 论 域 定义 为 {一 n ,一 x 十 1,…, 一 1,0,1,…,n 一 1,n); 将 U 的 论 域 定义 为 {一 7 ,一 /十 
,一 1 0,1 ,LO—1,7}. 

为 了 提高 实时 性 ,模糊 控制 器 常常 以 控制 查询 表 的 形式 出 现 。 该 表 反 映 了 通过 模糊 控制 
算法 求 出 的 模糊 控制 器 输入 量 和 输出 量 在 给 定 离散 点 上 的 对 应 关系 。 为 了 能 方便 地 产生 控制 


图 9.4 模糊 控制 器 的 基本 结构 
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查询 表 ,在 模糊 控制 器 的 设计 中 ,通常 就 把 语言 变量 的 论 域 定 义 为 有 限 整 数 的 离散 论 域 。 

@ 定义 各 语言 变量 的 语言 值 。 通 常 在 语言 变量 的 论 域 上 ,将 其 划分 为 有 限 的 几 类 。 例 
如 ,可 将 EE.EC 和 U 的 论 域 划 分 为 “正大 (PB)”“ 正 中 (PM)”“ 正 小 (PS)”“ 零 (ZO)”“ 负 小 
(NS)”“ 负 中 (NM)”“ 负 大 (NB)” 七 类 。 

类 别 越 多 ,规则 制定 灵活 ,规则 细致 ,但 规则 多 、 复 杂 , 编 制程 序 困难 ,占用 的 内 存 较 多 ; 类 
别 越 少 ,规则 越 少 ,规则 实现 方便 ,但 过 少 的 规则 会 使 控制 作用 粗糙 而 达 不 到 预期 的 效果 。 因 
此 ,在 选择 模糊 状态 时 ,要 兼顾 简单 性 和 控制 效果 。 

@ 定义 各 语言 值 的 隶属 函数 。 隶 属 函 数 决 定 着 模糊 集 的 模糊 性 。 设 计 合理 的 隶属 函数 
是 运用 模糊 理论 解决 实际 问题 的 基础 。 一 般 来 说 ,模糊 函数 的 确定 方法 有 例证 法 、 模 糊 统计 法 
和 专家 经 验 法 。 常 用 的 隶属 函数 的 类 型 有 三 角 型 . 梯 型 和 正 态 分 布 型 (高 斯 基 函 数 ) ,对 应 的 表 
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2) 规则 库 


模糊 规则 的 完善 程度 和 准确 程度 对 系统 的 作用 效果 起 决定 性 作用 。 模 糊 规则 库 由 若干 条 
控制 规则 组 成 ,这 些 控制 规则 根据 人 类 控制 专家 的 经 验 总 结 得 出 ,按照 IF*…is…ANDis… 
THEN…is… 的 形式 表达 ,如 图 9.5 所 示 。 

其 中 ,E、EC 分 别 是 输入 语言 变量 “误差 ”误差 
变化 率 ”; U 是 输出 语言 变量 “控制 量 ”; A,、B;、C; 是 R,: IF Eis A; AND EC is B, THEN Uis C» 
第 i 条 规则 中 与 已 .EC U 对 应 的 语言 值 。 A 


Ri: IF Eis A! AND EC is BTHEN Uis C, 


3) 模糊 逻辑 推理 

模糊 控制 的 实质 是 模糊 逻辑 推理 , 即 根 据 模 糊 输 R: IF Eis A, AND EC is B, THEN Ui 
入 和 规则 库 中 蕴涵 的 输入 输出 关系 ,通过 模糊 推理 方 图 9.5 规则 库 示 例 
法 得 到 模糊 控制 器 的 输出 模糊 值 。 

4) 清晰 化 接口 


由 模糊 推理 得 到 的 模糊 输出 值 C” 是 输出 论 域 上 的 模糊 子 集 ,只 有 其 转化 为 精确 控制 量 
u，, 才 能 施加 于 对 象 ,这 种 转化 的 方法 称 为 清晰 化 /去 模糊 化 /模糊 判决 。 模 糊 判 决 的 方法 有 很 
多 ,常用 的 有 最 大 隶属 度 法 .重心 法 和 取 中 位 数 法 。 这 里 只 对 重心 法 (也 叫 加 权 平 均 法 ) 做 简单 
介绍 。 

重心 法 对 模糊 输出 量 中 各 元 素 及 其 对 应 的 隶属 度 求 加 权 平均 值 ,并 进行 四 舍 五 人 取 整 ,来 
得 到 精确 输出 控制 量 ,表达 式 如 下 : 
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DUinc i 
WY - (9. 10) 
Soke CO 
式 中 ,〈 ) 代 表 四 舍 五 人 取 整 操作 。 
3. 模糊 查询 表 
模糊 控制 器 的 工作 过 程 如 下 : 


J@ 模糊 控制 器 实时 检测 系统 的 误差 和 误差 变化 率 e ”和 ec”; 

@ 通过 量化 因子 k。 和 ,将 e” 和 ec“ 分别 量 化 为 控制 器 的 精确 输入 E“* 和 EEC”; 

@ 下 和 EC 通过 模糊 化 接口 分 别 转化 为 模糊 输入 A 和 下 ; 

@ 将 A“” 和 B” 根据 规则 库 蕴 涵 的 模糊 关系 进行 模糊 推理 ,得 到 模糊 控制 输出 量 C”; 

@ 对 C”" 进行 清晰 化 处 理 , 得 到 控制 器 的 精确 输出 量 U”; 

@ 通过 比例 因子 , 将 U ”转化 为 实际 作用 于 控制 对 象 的 控制 量 wx” 。 

对 加 一 回 步 离线 进行 运算 ,对 于 每 一 种 可 能 出 现 的 已 和 EC 取 值 ,计算 出 相应 的 输出 量 
U ,并 以 表格 的 形式 储存 在 计算 机 内 存 中 ,这 样 的 表格 称 为 模糊 查询 表 。 

如 果 瓦 .EC 和 TU 的 论 域 均 为 { 一 6, 一 5, 一 4, 一 3, 一 2, 一 1,0,1,2,3,4,5,6}, 则 生成 的 模 
糊 查询 表 具 有 如 图 9. 6 所 示 的 形式 。 


和 
在 
i 


图 9.6 模糊 查询 表示 例 


综 上 所 述 ,模糊 控制 器 的 设计 内 容 主要 有 以 下 几 个 方面 : 
(1) 确定 模糊 控制 器 的 输入 变量 和 输出 变量 ; 

(2) 确定 输入 、 输 出 的 论 域 和 天 。 开 。 -K, 的 值 ; 

(3) 确定 各 变量 的 语言 取 值 及 其 隶属 函数 ; 

(4) 总 结 专 家 控制 规则 及 其 蕴涵 的 模糊 关系 ; 

(5) 选择 推理 算法 ，; 

(6) 确定 清晰 化 的 方法 ; 

(7) 总 结 模糊 查询 表 。 
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9.1.3 学 习 控 制 

学 习 控 制 是 指 对 于 具有 可 重复 性 的 被 控 对 象 ,利用 控制 系统 先前 的 控制 经 验 , 根 据 测 量 系 
统 的 实际 输出 信号 和 期 望 信号 ,寻找 一 个 理想 的 输入 输出 特性 曲线 ,使 被 控 对 象 产 生 期 望 运 
动 。 这 种 控制 方法 能 够 在 系统 进行 过 程 中 估计 未 知 信息 ,并 据 之 进行 最 优 控制 ,以 便 逐 步 改善 
系统 性 能 。 一 个 学 习 系统 通过 所 学 得 的 信息 以 控制 某 个 具有 未 知 特征 的 过 程 , 则 称 该 系统 为 
学 习 控 制 系统 。 

1. 学 习 控 制 的 数学 描述 

在 有 限时 间 域 [0,T] 内 ,给 出 受 控 对 象 的 期 望 响应 yj (1) ,寻求 某 个 给 定 输入 ui (1) ,使 得 
u(t) 的 响应 yx (z) 在 某 种 意义 上 获得 改善 ,其 中 ,k 为 搜索 次 数 ,上 E[0,T], 则 称 该 搜索 过 程 
为 学 习 控 制 过 程 。 当 有 一 co 时 ,满足 yj (1) 习 yy (1) , 则 该 学 习 控制 过 程 是 收敛 的 。 

2. 学 习 控制 系统 的 运行 方式 

学 习 控制 系统 有 两 种 运行 方式 : 启动 学 习 和 运行 学 习 。 启 动 学 习 是 指控 制 器 启动 后 初始 
运行 的 学 习 。 它 反复 依据 当前 的 特征 状态 .前段 运行 效果 的 特征 记忆 以 及 相应 的 学 习 规则 , 确 
定 运行 决策 。 运 行 学 习 是 指 在 控制 运行 中 对 象 类 型 变化 时 的 学 习 过 程 ,通过 尝试 考虑 所 有 可 
能 的 决策 ,修改 控制 策略 和 控制 参数 。 

3. 学 习 控 制 与 常规 自 适应 控制 的 异同 

这 两 种 控制 方法 的 主要 相同 之 处 有 : 

(1) 学 习 系 统 是 自 适应 系统 的 发 展 与 延伸 , 它 能 够 按照 运行 过 程 中 的 “经 验 ” 和 “教训 ”来 
不 断 改进 算法 ,增长 知识 ,更 广泛 地 模拟 高 级 推理 .决策 和 识别 等 人 类 的 优良 行为 和 功能 ; 

(2) 都 是 解决 系统 不 确定 性 问题 的 方法 ; 

(3) 都 是 基于 在 线 的 参数 调整 算法 ; 

(4) 都 使 用 与 环境 、 对 象 闭环 交互 得 到 的 信息 。 

它们 还 有 一 些 不 同 点 ,主要 体现 在 以 下 几 个 方面 。 

(1) 自 适应 控制 系统 在 未 知 环境 下 的 控制 决策 是 有 条 件 的 ,其 控制 算法 依赖 于 受 控 对 象 
数学 模型 的 精确 辨识 ,并 要 求 对 象 或 环境 的 参数 和 结构 能 够 发 生 大 范围 突变 。 这 就 要 求 控制 
器 有 较 强 的 适应 性 、 实 时 性 ,并 保持 良好 的 控制 品质 。 在 这 种 情况 下 , 自 适应 控制 算法 将 变 得 
过 于 复杂 ,计算 工作 量 大 ,而 且 难 于 满足 实时 性 和 其 他 控制 要 求 。 因 此 , 自 适应 控制 的 应 用 范 
围 比较 有 限 。 

(2) 自 适应 控制 着 眼 于 瞬时 观点 ,缺乏 记忆 ，。 

(3) 当 受 控 对 象 的 运动 具有 可 重复 性 时 , 即 受 控制 系统 每 次 进行 同样 的 工作 时 ,就 可 把 学 
习 控 制 用 于 该 对 象 。 在 学 习 控 制 过 程 中 ,只 需要 检测 实际 输出 信号 和 期 望 信号 ,而 受 控 对 象 复 
杂 的 动态 描述 计算 和 参数 估计 可 被 简化 或 被 省 略 。 

(4) 学 习 控 制 强调 经 验 和 记忆 。 

4. 一 般 学 习 控制 系统 的 组 成 

一 般 学 习 控 制 系统 主要 由 常规 反馈 控制 环 ( 即 先 验 补偿 器 )、 自 适应 环 和 学 习 环 组 成 ， 如 
图 9.7 所 示 。 

学 习 控 制 系统 从 结构 上 可 以 分 为 基于 模式 识别 的 学 习 控 制 、 迭 代 学 习 控 制 、 重 复学 习 控 制 
和 基于 神经 网 络 的 学 习 控制 。 其 中 ,基于 模式 识别 的 学 习 控 制 就 是 用 模式 识别 方法 对 输入 信 
息 进 行 提取 和 人 处理, 为 控制 决策 和 学 习 适 应 提供 依据 。 和 迭代 学 习 控制 则 是 反复 应 用 系统 以 前 
运行 得 到 的 信息 ,以 获得 能 够 产生 期 望 输出 轨迹 的 控制 输入 ,改善 控制 质量 。 重 复学 习 控 制 是 
根据 内 模 原理 ,引入 能 够 产生 周期 信号 的 重复 补偿 器 ,以 跟踪 具有 周期 性 的 任意 目标 信号 。 基 
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图 9.7 学 习 控 制 系统 的 组 成 


于 神经 网 络 的 学 习 控 制 是 以 神经 网 络 为 辨识 模型 或 控制 器 ,神经 网 络 的 学 习 训 练 算法 是 该 控 
制 方案 的 关键 。 


9.1.4 径 向 基 荡 数 神经 网 络 

径 向 基 函 数 神 经 网 络 (Radial Basis Function Neural Network,RBFNN) 在 到 近 能 力 .分 类 
能 力 和 学 习 速 度 等 方面 都 优 于 BP 神经 网 络 , 其 结 
构 简单 .训练 简洁 .学习 收 敛 速度 快 ,能 够 逼近 任 
意 非 线 性 函数 ,克服 局 部 极 小 值 问题 。RBFNN 是 
一 种 前 馈 式 的 神经 网 络 ,由 三 层 神经 元 组 成 ,分 别 
是 输入 层 、 隐 含 层 以 及 输出 层 , 如 图 9. 8 所 示 。 从 
输入 层 到 隐 含 层 的 权 值 为 1; 输出 层 是 对 线性 权 
值 进 行 调整 ,采用 线性 优化 策略 ; 隐 含 层 使 用 径 向 
基 范 数 对 输入 层 极 性 变换 升 维 , 径 向 基 范 数 是 一 
个 取 值 仅仅 依赖 于 离 原 点 距离 的 实 值 函 数 , 采 用 图 9.8 径 向 基 函 数 神经 网 络 结构 图 
非 线性 优化 策略 。 

1. 基本 思想 

RBFNN 的 基本 思想 是 : 以 径 向 基 函 数 作 为 隐 含 层 单元 的 “ 核 ?%(z) 构 成 隐藏 空间 , 隐 含 
层 对 输入 矢量 进行 变换 ,将 低 维 的 输入 映射 到 高 维 空间 中 ,并 在 高 维 空间 中 进行 加 权 线 性 组 合 
( 即 网 络 输出 是 隐 含 层 单元 输出 的 线性 加 权 和 ,该 权 值 即 为 网 络 需 要 训练 学 习 的 参数 ) ,从 而 使 
整个 系统 架构 达到 拟 合 非 线性 函数 的 目的 。 当 径 向 基 函 数 的 中 心 点 确定 后 ,这 种 映射 关系 也 
就 确定 了 。 当 输入 一 个 变量 时 ,由 于 径 向 基 函 数 的 作用 ,在 距离 某 一 个 中 心 比 较 近 的 隐 含 单元 
输出 较 大 ,其 他 远离 输入 变量 的 隐 含 单元 输出 较 小 ,因此 调整 权 值 时 ,可 看 作 只 有 部 分 权 值 有 
响应 ,这 样 的 局 部 响应 机 制 使 得 径 向 基 函 数 神经 网 络 的 训练 学 习 变 得 简洁 , 且 避 免 了 局 部 极 小 
值 的 问题 。 

RBFNN 需要 调整 的 参数 主要 包括 : @ 径 向 基 函 数 神经 网 络 中 心 的 选取 ; @ 径 向 基 函 数 
神经 网 络 中 心 的 宽度 选取 ; 图 中 心 点 数目 ( 即 隐 含 层 的 隐 含 单元 数目 ) 。 

2. 数学 描述 

当 需 要 使 用 径 向 基 函 数 网 络 去 盘 近 一 个 位 置 的 函数 瑟 (z)ER",zER” 时 ,其 数学 表达 
式 如 下 : 


H(zx)=W'® (zx) (9 汀 ] 功 
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其 中 ,W=[w1 os por] ER” ,wi ,ws “,w,ER" 是 径 向 神经 网 络 隐 含 层 到 输出 层 对 应 
的 权 值 矩阵 ;名 de ,0) a 是 一 个 回归 函数 矢量 ,其 中 /为 隐藏 层 节 点 个 


| 二 | 
一 一 一 | (C9; 12) 


Ns 
其 中 ,c; ER” 和 o; ER 分 别 是 径 向 基 函 数 的 中 心 和 宽度 。 


为 使 证 明 过 程 表 述 更 加 简洁 ,这 里 重新 定义 了 与 式 (9. 12) 等 价 的 表达 式 : 


H(zx)=@®' (zx)0 (9. 13) 
其 中 ,0: col (wi,ws ,ww 1)ER”,m 二 n* lL,col(a ,06,…,n) 表 示 向 量 a,6,…,n 构成 列 向 量 
的 算 子 ,o; 王 [oil yoiz oo ER ,i=1,2,. ,lB (zr)=diag(®1 ,8B, ,8B, ),diag(* ) 
表示 对 角 和 矩 阵 算 子 ,B; 二 [Oj (zx),@O,(z),…,D, (XN "ERY ,i=1,2,.…,n 


9.2 基于 输出 受 限 的 自 适 应 控制 器 的 机 械 璧 参考 轨迹 调节 


随 着 中 国 劳动 力 成 本 的 不 断 上 升 ,机 器 人 自动 化 逐渐 成 为 了 中 小 企业 生产 或 服务 型 行业 
的 劳动 力 解决 方案 。 近 十 年 来 ,机 器 人 的 应 用 也 越 来 越 广泛 ,如 生产 流水 线 上 的 工业 机 器 人 、 
与 工人 一 起 协作 完成 生产 任务 的 协作 机 器 人 、 和 餐饮 业 中 服务 型 的 送 餐 机 器 人 、 医 学 上 的 护理 康 
复 机 器 人 等 。 对 于 中 小 企业 来 说 ,产品 周期 较 短 ,订单 样式 多 ,如 3C 产业 中 手机 壳 的 加 工 , 有 
小 批量 .定制 化 . 短 周期 的 特点 。 目 前 大 部 分 工业 机 器 人 仅 能 重复 固定 单一 的 动作 , 产 线 更 蔡 
需要 花费 大 量 时 间 对 机 器 人 进行 重新 编程 ,此 类 机 器 人 已 经 不 能 够 满足 中 小 企业 的 需求 。 因 
此 当前 市 场 要 求 机 器 人 更 具 适 应 性 、 安 全 性 .小 型 化 和 轻 量 化 。 简 言 之 ,机 器 人 与 人 和 环境 交 
互 的 需求 变 得 更 加 迫切 。 


9.2.1 背景 

1. 概述 

众所周知 ,对 于 一 个 简单 的 动作 (如 手 握 水 杯 ), 人 类 能 够 轻松 地 完成 ,而 机 器 人 完成 这 个 
动作 则 需要 研究 人 员 花 费 数 年 的 时 间 来 实现 这 项 技术 。 为 了 让 机 器 人 像 人 类 一 样 能 够 灵活 自 
如 地 控制 其 手臂 ,研究 者 们 在 仿生 学 上 做 出 了 大 量 的 研究 。 研 究 人 员 根 据 人 类 中 枢 神 经 
(Central Nervous System,CNS) 控 制 手臂 运动 的 机 制 提出 了 各 种 猜想 假设 。 这 些 猜 想 假 设 启 
发 了 人 们 设计 人 类 仿生 的 控制 器 ,使 得 机 器 人 能 够 更 加 灵活 、 智 能 地 与 人 /环境 进行 交互 。 因 
此 ,本 节 通 过 研究 人 类 CNS 对 与 环境 交互 时 的 运动 控制 机 制 ,设计 一 款 性 能 与 人 类 相近 甚至 
更 优 的 仿生 自 适应 控制 器 。 

2. 机 械 臂 轨迹 跟踪 发 展现 状 

机 器 人 在 某 些 应 用 领域 ,如 老年 人 护理 、 健 康 护理 .医疗 康 复 . 人 机 协作 等 方面 ,都 需要 机 
器 人 与 环境 /人 进行 交互 ,由 于 各 种 各 样 的 因素 ,这 种 交互 存在 许多 不 确定 性 。 为 了 更 好 地 控 
制 机 器 人 与 人 /环境 进行 交互 ,人 们 试图 通过 研究 人 类 自身 CNS 运动 控制 机 制 来 启发 .指导 机 
器 人 轨迹 跟踪 控制 器 的 设计 。 

关于 机 器 人 轨迹 跟踪 控制 器 的 研究 ,有 学 者 分 析 人 类 实验 者 用 手臂 拖 动 一 个 能 够 产生 模 
拟 力 场 的 平面 机 械 辟 手柄 跟踪 给 定 的 点 对 点 轨迹 ,在 有 力 场 作用 下 的 轨迹 偏离 情况 ,揭示 了 人 
类 CNS 会 在 重复 的 运动 中 通过 手臂 的 肌肉 收缩 舒张 ,逐渐 学 习 调 整 施加 力 的 大 小 。 也 有 研究 
利用 触觉 来 识别 力 场 表 面 ,该 研究 提 到 了 CNS 在 运动 过 程 中 具有 两 种 自 适应 响应 ,一 种 是 对 
力 进 行 抑制 ( 即 尽量 跟踪 参考 轨迹 ) , 另 一 种 是 沿 着 物体 边界 运动 ( 即 修改 原 参考 轨迹 以 减少 交 
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互 力 ) 。 在 研究 这 两 种 响应 的 切换 方式 的 基础 上 ,该 研究 提出 CNS 通过 维持 与 环境 的 交互 力 
不 变 的 机 制 来 选择 这 两 种 自 适应 响应 ,从 而 使 两 种 响应 在 切换 过 程 中 更 加 顺 滑 。 可 以 看 出 ,人 
类 在 运动 过 程 中 受到 力 的 扰动 时 ,会 通过 修改 运动 轨迹 来 减少 受到 的 力 对 运动 的 影响 。 这 与 
Hogan 首次 提出 的 阻抗 控制 相 契 合 一 一 阻抗 控制 的 基本 思想 就 是 牺牲 一 定 的 控制 精度 ,以 调 
整 力 和 轨迹 的 动态 关系 为 控制 目标 ,控制 机 器 人 完成 既定 任务 。 因 此 ,在 后 续 的 研究 中 ,人 们 
一 般 都 会 用 到 阻抗 控制 。 


9.2.2 轨迹 跟踪 控制 器 设计 

1. 机 械 辟 运动 学 与 动力 学 模型 

机 械 臂 的 控制 主要 是 通过 给 各 个 关节 驱动 的 力矩 ,使 得 机 械 臂 的 各 个 关节 位 置 、 速 度 和 加 
“速度 等 状态 变量 尽量 与 参考 轨迹 重合 ,从 而 完成 预期 的 任务 。 在 控制 机 械 臂 时 ,往往 需要 建立 
数学 模型 以 表述 其 动态 特性 。 机 械 臂 的 数学 模型 由 两 部 分 组 成 ,分 别 是 运动 学 模型 和 动力 学 
模型 。 

1) 机 械 臂 的 运动 学 模型 

机 械 臂 末端 执行 器 正 问 运 动 学 方程 为 : 

z=V(g), dg ER’,rz ER” (9.14) 

其 中 ,zx 表示 机 械 臂 末端 执行 器 在 笛 卡 儿 空 间 中 的 位 置 和 方向 ; n 表示 机 械 臂 有 nn 个 关节 ; gq 
为 对 机 械 臂 各 个 关节 在 关节 空间 的 配置 ; m 表示 机 械 臂 的 自由 度 。 更 (，) 为 机 械 臂 的 正 向 运 
动 学 模型 , 单 向 连接 了 关节 空间 和 任务 空间 ,在 指导 关节 空间 配置 的 前 提 下 ,可 由 模型 计算 出 
末端 执行 器 在 任务 空间 中 的 位 置 。 

机 械 臂 逆向 运动 学 的 数学 模型 为 : 


q=YV (xz), g ER",r ER” (9. 15) 

机 械 臂 的 雅 可 比 矩 阵 Je) 将 任务 空间 中 的 力 与 关节 空间 中 的 力矩 相 联系 : 
t=J(g)'F (9. 16) 
其 中 ,下 ER 有 ”为 机 械 臂 各 个 自由 度 的 力 ,z GE 及" 为 力作 用 下 对 应 产生 的 关节 空间 的 力矩 ， 


J(g)ER””", 
2) 机 械 臂 的 动力 学 模型 
考虑 一 个 具有 nn 个 自由 度 的 机 械 辟 , 其 动态 特性 可 以 用 如 下 二 阶 非 线 性 微分 方程 描述 : 
M(gD)I+C(g 0 +G(g) tr 一 r， (9.17) 
其 中 ,g ER” 为 关节 空间 配置 量 ,M (gq) ERR"” 为 机 械 臂 的 惯性 矩阵 ,CC(g ,9)ER"” 表 示 机 
械 臂 的 离心 力 和 科 氏 力 ,G(g)ERR” 为 由 于 重力 引起 的 力矩 ,ry €E 恨 ”为 与 环境 扰动 相关 的 力 
和 矩 ,t, EE 及 ”为 控制 器 输出 的 控制 力矩 。 
机 械 臂 在 动力 学 方面 具有 以 下 常用 性 质 , 这 些 性 质 对 控制 器 的 设计 具有 很 大 帮助 。 
性 质 1: M(a ) 是 对 称 正 定 和 矩阵, 和 ms 为 常量 0 二 mj 过 ms, ,EERR" ,满足 : 
milél*<E MOVESm, lel’ 
性 质 2: M(g) 一 2C(q ,4) 为 斜 对 称 和 矩阵 , 且 满 足 V&,éT(M (gq) 一 2C(q ,4))# 一 0。 
性 质 3: 存在 依赖 于 机 械 臂 参数 的 参数 向 量 卫 ， 使 得 M(g)、.C(g,9)、G(g) 满 足 线 性 关系 : 
M(g)ai+C(g,9)0+G(gqg)=®B(g,9,4a,0)P 
其 中 ,® (g ,4 ,a,6)ERR"””” 为 回归 和 矩阵 , 且 为 已 知 和 矩阵 ; PE 及 ”是 描述 机 器 人 质量 特性 的 未 
知 、 定 常 参数 向 量 。 
性 质 4: M(g).C(g,g)、G(g),Vg,9 ER 有 "一 阶 连续 可 导 。 
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2. 问题 描述 
机 械 辟 跟踪 运动 时 受到 的 环境 扰动 ,例如 机 械 辟 运动 时 遇 到 障碍 物 时 ,或 者 是 一 个 较为 柔 
软 的 物质 如 果 没 有 调整 自身 的 阻抗 ,或 者 没有 修改 自身 的 轨迹 ,那么 末 段 执行 器 受到 的 环境 作 
用 力 将 会 变 得 很 大 ,机 械 辟 很 可 能 会 受到 损坏 。 考虑 任务 轨迹 为 直线 的 情况 ,对 环境 力 F。、 
和 跟踪 偏离 面积 RDA( 即 Reaching Deviation Area) 的 描述 如 下 : 
1 


Ws | Fa (9. 18) 
tp to 


RDA=|" xcz)dz (9. 19) 


其 中 ,Fi(z) 表 示 环 境 作用 于 机 械 臂 的 时 变 力 , 它 与 机 械 臂 运动 轨迹 相关 ; X (xz) 表示 轨迹 偏离 
量 ,zE 有 表示 当前 机 械 臂 沿 着 x 轴 运 动 ; RDA 为 偏离 x 轴 的 面积 。 

假设 机 械 臂 的 末端 执行 器 需要 跟踪 从 A 点 到 吾 总 的 点 到 点 直线 任务 轨迹 ,跟踪 的 同时 ， 
受到 一 个 以 点 A 到 B 的 直线 长 度 为 直径 ,中 心 为 圆心 的 辐射 状 力 场 的 作用 ,如 图 9. 9 所 示 。 
力 场 的 表达 式 为 : 
kro— rns TE 


a 


e 


(9:202 


Os P'S 
其 中 ,FF。、k,、ro\r\n 分 别 表示 交互 力 、 环 境 刚 度 、 辐 射 力 半径 .末端 执 行 器 离 圆 心 的 距离 和 
力 的 作用 方向 矢量 。 目 前 研究 较 多 的 是 使 用 选 
代 学 习 的 方法 ,通过 一 次 又 一 次 的 试验 ,记录 误 
差 ,更 改 轨迹 以 用 于 下 一 次 试验 作为 参考 轨迹 ， 
最 终 收敛 为 能 够 平衡 下 ,,, 和 RDA 的 最 佳 轨迹 。 
如 图 9. 9(a) 所 示 , 在 最 初 跟 踪 1 线 的 任务 轨迹 
( 记 为 lstTraj.) 的 时 候 生 成 了 2 线 ( 记 为 
2ndTraj. ) ,跟踪 2ndTraj. 的 同时 生成 了 3 线 的 参 
考 轨迹 ( 记 为 3rdTraj. ) ……. 以 此 类 推 ,通过 轨迹 
的 不 断 迭 代 更 新 ,最 后 生成 一 条 平衡 环境 力 与 偏 
离 面积 的 最 佳 轨迹 。 图 9.9 轨迹 自 适应 策略 示意 图 

然而 ,上 述 方法 需要 在 离线 生成 一 条 轨迹 之 后 ,机 械 辟 再 去 跟踪 。 在 首次 迭代 的 时 候 , 如 
果 力 场 的 刚度 非常 大 的 情况 下 ,将 很 可 能 导致 机 械 辟 损坏。 因此 需要 一 种 能 够 在 线 自 适应 调 
节 参 考 轨 迹 的 算法 ,从 而 避免 上 述 问题 。 解 决 方法 是 先 对 环境 建 模 ,利用 基于 李 雅 普 诺 夫 函 数 
的 方法 进行 参数 估计 ,通过 估计 的 模型 预测 出 下 一 时 刻 环境 作用 力 的 大 小 ,以 此 为 依据 ,对 下 
一 刻 的 跟踪 参考 点 进行 修改 , 如 图 9. 9(b) 所 示 , 在 初始 时 刻 跟 从 参考 点 即 1 点 ( 记 为 
lstPoint); 根据 对 环境 参数 的 估计 ,生成 第 2 个 采样 时 刻 的 跟踪 参考 点 即 2 点 ( 记 为 
2ndPoint) ; 根据 第 2 个 采样 时 刻 对 环境 的 估计 ,生成 第 3 个 采样 时 刻 的 跟踪 参考 点 即 3 点 ( 记 
为 3rdPoint)……… 以 此 类 推 ,最 终 这 些 参 考点 连 成 一 条 自 适应 轨迹 。 该 轨迹 的 目标 如 下 : 


T= ey -一 -| Froriu 十 | ”xTRXdz (9. 21) 
区 t * * 
从 任意 t 时刻 开始 ,对 参考 轨迹 的 修改 应 能 够 保证 当前 时 刻 t 到 最 后 时 刻 zy 的 损耗 函数 J 


最 小 。 
根据 上 述 的 分 析 , 机 械 辟 的 阻抗 . 力 ,参考 轨迹 在 线 调节 技术 实际 上 由 两 个 嵌 套 的 闭环 构 
成 ,内 环 是 一 个 位 置 跟踪 环 ,要 求 控 制 器 能 够 准确 跟踪 给 定 的 轨迹 ; 外 环 是 一 个 参考 轨迹 自 适 
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应 调节 絮 , 要 求 能 够 根据 机 械 辟 与 环境 相互 作用 反馈 回来 的 信息 自 适 应 地 修改 原 有 参考 轨迹 ， 
其 输出 作为 内 环 轨迹 跟踪 的 输入 ,如 图 9. 10 所 示 。 考 虑 到 这 两 个 闭环 是 相互 影响 的 ,控制 器 
的 跟踪 效果 将 会 大 大 地 影响 到 机 械 臂 与 环境 相互 作用 的 表现 ,从 而 影响 到 轨迹 自 适 应 模块 的 
性 能 ,因此 要 设计 一 个 如 上 所 述 的 控制 系统 ,首先 需要 有 一 个 能 够 准确 跟踪 轨迹 的 轨迹 跟踪 控 
制 器 ,其 次 需要 一 个 能 够 根据 环境 状态 以 自 适 应 地 修改 轨迹 的 轨迹 自 适 应 模块 。 下 面 将 分 别 
对 这 两 部 分 设计 进行 详细 的 阐述 。 


Xdesired e 
Edie 控制 器 Re | | 


图 9.10 系统 总 体 结构 图 


3. 轨迹 跟踪 器 设计 
机 械 臂 轨迹 跟踪 控制 器 的 最 基本 目标 是 控制 输出 能 够 准确 地 跟踪 控制 器 输入 参考 轨迹 ， 
并 使 系统 具有 一 定 的 鲁 棱 性 ,能 够 抵抗 外 界 的 干扰 。 本 小 节 分 别 设计 了 基于 边界 估计 的 滑 模 
控制 器 和 输出 受 限 的 自 适应 控制 器 ,这 两 种 控制 器 将 会 在 仿真 中 比较 其 控制 性 能 ,并 选 定 使 用 
其 中 一 种 控制 器 构成 机 械 辟 的 力 、 阻 抗 参考 轨迹 自 适 应 的 控制 系统 。 
在 进行 设计 前 ,需要 应 用 到 以 下 假设 和 引 理 。 
假设 系统 受到 的 扰动 有 界 并 满足 不 等 式 : 
lz <ado+ailiel++a,lell (9. 22) 
其 中 ,d, 这 0(i=0,1;2)ye 二 gg 一 gy。 
引 理 ” 李 雅 普 诺 夫 函数 V(t) 是 正定 的 ,在 初始 状态 V(0) 有 界 , 且 V() 一 阶 连续 可 导 的 情 
况 下 ,如 果 下 列 不 等 式 成 立 : 
Ve BV 《9.239 
其 中 ,o 二 0,c>0, 那 么 多 有 界 。 
1) 基于 边界 估计 的 滑 模 控制 器 
本 部 分 结合 边界 估计 方法 和 径 向 基 函 数 神经 网 络 ,设计 一 个 能 够 对 扰动 边界 进行 估计 的 
滑 模 控制 器 。 该 控制 器 将 机 械 臂 的 动力 学 模型 分 解 为 两 部 分 ,一 部 分 是 系统 自身 的 结构 模型 ， 
另 一 部 分 是 外 界 未 知 的 干扰 ,系统 参数 估计 误差 等 不 确定 项 。 整 个 控制 器 的 控制 框图 如 图 9. 11 
所 示 。 


逆 运 gtask 本 
控制 器 | “| 也 相生 
有 界 性 sy 
| 
图 9. 11 基于 边界 估计 的 滑 模 控制 器 的 控制 框图 
构造 滑 模 算 子 ; 和 辅助 变量 g, ,s 二 十 TT,,9, 二 44 一 了 ,其 中 ov 为 期 望 角度 ,对 系统 模 
型 进行 分 析 ;、 
二 人 
=M(D TC 6s FM Cd 于 下 6) 
(9.24) 
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则 机 械 臂 动力 学 模型 即 式 (9. 17) 可 以 改写 为 : 
Mo) 二 C(a 0o)s 十 百 (qao do ) 十 ry =7, (9. 253 
其 中 ,H(g,g,9,,9,) 二 M(g)g,+C(g,9)g,+G(g)。 
初 选 李 雅 普 诺 夫 函数 ， Vi 一 亡 s"Mss 结合 机 械 辟 性 质 2 和 式 (9. 25), 对 其 求 导 后 进 生 
分 析 : 
本 一 本 Ts 二 s TM;=s (Mi+Cs)=s (rt, 一 rz 一 有 (go 0 D)) (9.26) 
设计 控制 器 如 下 : 
ka i (9.27) 
该 控制 器 由 三 项 组 成 ,rs 为 允 近 项 ,用 于 允 近 并 补偿 系统 的 非 线 性 部 分 ; t+, 为 进行 边界 估计 
后 的 滑 模 项 ,用 于 补偿 系统 执行 轨迹 跟踪 任务 时 受到 来 自 环境 、 系 统 内 部 等 不 确定 的 扰动 ; 将 


滑 模 算 子 展开 后 可 知 ,rj 项 等 价 于 一 个 PD 控制 器 ,用 于 保证 系统 的 稳定 性 。 式 (9. 25) 中 
各 项 的 具体 表达 式 如 下 : 


trap = A (ggg 0,) 
r, 一 一 6sgn(s) (9. 28) 
区 一 下，s 光 ， = 十 ko 
其 中 ,k。 用 于 保证 系统 处 于 初始 状态 ,和 , 为 调节 到 最 佳 值 之 前 的 稳 态 。 
分 析 待 允 近 项 H(g ,9 ,9,,9,), 考 虑 到 gg 二 e 十 qs , 式 (9. 21) 变 换 如 下 : 
H(q,g,94,94;9a) =M(etqa) (da —TE)+C(e tqae ta) (Gy +T,) +G(et qa) 
=M(ga)ds +C(qa da da +G(qs) +e, 
=H,y (da ,90414) +e, (9. 29) 
其 中 ,e, 一 H(g,9,94,94,44) 一 Hy (94 ,94 qz), 当 且 仅 当 系统 完全 跟踪 到 期 望 的 轨迹 时 ， 
让 一直 ,( 即 :一 2 一 一 0) 。 为 了 设计 一 个 高 效 的 控制 器 ,需要 减少 神经 网 络 的 节点 数 ,这 里 使 
用 径 向 基 函 数 神经 网 络 逼 近 让 , 项 ,而 不 是 站 项 ,使 得 神经 网 络 输入 结 点 数 从 5n 减少 至 3n， 
从 而 大 大 减少 了 神经 网 络 隐藏 结 点 数目 。 其 中 
5 二 全 (9. 30) 
设 通 近 误差 为 ex 一刀 ,一 让 , 。 假 设 逼 近 误差 及 系统 跟踪 误差 导致 的 模型 误差 均 有 界 , | e, | 入 
zi, 上 es | 入 zw，。 重 新 定义 式 (9. 21) ,zy 一 r, 十 ri ,将 因 跟 踪 误差 及 逼近 误差 产生 的 不 确定 
但 有 界 项 s, .se, 统一 视 为 常量 扰动 。 
laa tntia taotatleil ta lel gao 4a lel+alel 
do 一 rdl 十 rdz 十 do 
式 (9. 24) 中 各 项 的 具体 表达 式 与 更 新 规律 如 下 : 
W = 6sT 
[ae sl (9. 31) 
k, =élsl’ 
观察 式 (9. 24) 中 最 后 一 个 等 式 , 对 扰动 项 进行 分 析 
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l= 上 本 二 路 记 测 委 ee 册 寺 A el : 
一 sIr lsl(do+ailel+t+a,lel) (9. 32) 
委 dolsl+ealsl + lsl lelkeiaAnasC) 十 dz) 
选择 李 雅 普 诺 夫 函 数 如 下 : 
1 和 
Vs Ts 十 寺村 十 je + 志 IWwl (9. 33) 


三 Wn 


其 中 , | 钨 ?= a; =tr(WIW) ,Wm=W’ —W。 
1 一 1 三 1 
结合 式 (9. 25) 和 式 (9. 32) ,对 式 (9. 32) 关 于 时 间 求 导 可 得 : 
V=sT (MS 十 Cs) HE,k, /E+od/¢ + tr WW)/y 


a 一 r, WE —ry —W’B—e, —e,) th,k, /Eod/t + tr (WW)/ 


<—sTry—pk, ls :molsl tt Bs WHWIW/y) 
=—(0— do ) | sl = 
+ | 
一 | (9. 34) 
2 
J 下 
9 Ga 9 9 压 ， 口 
其 中 ,@ Wt ; 满足 c>du , 且 0 为 正定 矩阵 时 ,可 知 清 
区 


模 算 子 ; 逐渐 趋 近 于 0; 考虑 到 ;= 二 十 Te , 则 跟踪 误差 将 逐渐 趋 近 于 0。 
由 于 控制 器 中 存在 符号 函数 相关 项 ,容易 引起 输出 控制 扭矩 的 震颤 ,需要 对 机 械 臂 输出 控 
制 量 进行 除 额 。 震 颤 问题 可 以 使 用 边界 理论 进行 消除 ,使 用 sat() 代 替 sgn(): 


Ts 
s/b—1 寺 s/b 1 
sat(s) = 


(9. 35) 
Ss 
ee 
其 中 ,2 为 边界 层 厚 度 。 因 此 ,最 终 基 于 边界 控制 的 滑 模 控制 器 为 : 
rs 一 全 1G6 一 人 sat(s) —hk,s (9. 36) 


2) 输出 受 限 的 自 适 应 控制 器 

输出 受 限 控制 器 的 设计 目标 是 : 在 输出 状态 受到 约束 且 环 境 干扰 作用 于 机 械 臂 时 ,仍然 
能 够 跟踪 轨迹 ,上 且 不 违背 约束 条 件 。 在 机 器 人 应 用 中 ,机 器 人 的 运动 需要 受到 某 些 特定 条 件 的 
约束 ,如 关节 角度 .关节 角速度 和 关节 角 加 速度 的 约束 ,以 及 在 与 人 交互 过 程 中 的 安全 性 要 求 ， 
保护 机 械 辟 不 在 运动 过 程 中 损坏 等 。 因 此 ,状态 受 限 控制 器 是 一 个 值得 深入 研究 的 领域 。 参 
考 利 用 障碍 李 雅 普 诺 夫 函 数 的 设计 方法 ,结合 径 向 基 函 数 神经 网 络 的 简化 方法 ,设计 一 个 输出 
受 限 控制 器 ,使 用 径 向 基 函 数 神经 网 络 来 通 近 系统 的 不 确定 项 ,利用 障碍 李 雅 普 诺 夫 函 数 在 关 
节 空 间 中 进行 控制 器 的 设计 使 输出 受 限 ,整个 控制 器 的 结构 如 图 9. 12 所 示 。 

输出 状态 的 受 限 不 等 式 为 |g; | 二 k,; ,Vi 二 0 一 1,2,…,72 ,其 中 &; 二 0。 对 机 械 臂 动力 学 
模型 的 表达 式 即 式 (9. 17) 进 行 变 换 , 其 在 状态 空间 中 表示 如 下 : 
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图 9.12 输出 受 限 的 控制 器 结构 


一 工 ? 
i,=M (gqg)[rt—ry —C(g,9)9 —G(g)] 


其 中 ,zi :一 azy: 一 0。 
定义 误差 信号 : 
. X11 Xa 


€2 = — PI 


(9.37) 


(9. 38) 


其 中 ,x 二 gq4 为 期 望 的 轨迹 ,8, 为 辅助 虚拟 的 参考 变量 ,有 待 进一步 设计 。 由 上 式 可 知 ,关于 


&, 的 误差 动态 方程 可 表示 如 下 : 
ME 十 Ce 一 rz 一 MP —CB—G++ry 
其 中 ,M、C、G 分 别 为 M(g)、C(g,9)、G(g) 的 缩写 。 


C1) 假设 系统 参数 已 知 ,考虑 到 约束 条 件 |&1， 加 靖宇 2 弘 入 


李 雅 普 诺 夫 函 数 : 
Ci Ti 1 i 
Wt = tan (BR )+ Me 
对 上 式 求 关 于 时 间 的 导数 可 得 
Vi()= > 人 ae + ET(Mé, + Cé,) 
i=1 2 neél; 
COS 全 


ei (éss 寺 a i) 
= Shu TP TCM + Ce,) 


n = n ; 一些 . 
se Bi 


2 
= 2 2 f= ey Nnél; 


为 了 构建 如 引 理 所 示 的 结构 ,设计 如 下 zt 和 pi: 


(9..39) 


» nn ,选择 tan 类 型 的 障碍 


(9. 40) 


E+el(rtiry — (MB + CB 十 G)) 


(9.41) 
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站 一 二 本 十 之 ， (9. 42) 
k n 本 E nn 
sin (SY ) cos (FE) 
ln Zk 2k., 
E11 
二 
COS 2 
2R-1 
ri 一 一 : —K,és 一 zz 十 MA 十 cp 十 G (9. 43) 
Ci 
‘( 窜 ) 
COS 2 
2k 


其 中 ,K=diag(k] sR, »"" sk,),K, =—=diaglk, ,Rk ,2 加 "ky ) ,下 和 五 ， 为 正定 常 对 角 和 矩阵 。 
将 式 (9.43) 和 式 (9. 42) 代 入 式 (9.41) 中 ,可 得 


2 
A tan 人 2 本 天 二 一 we) (9. 44) 


i 一 1 ci 
其 中 ,pi 之 0, 且 满足 pi 二 min (k; ,2X4min (Ks)/Amax CM)) ,i 二 1,2,…,n, 因 此 Vi(t) 为 负 定 , 保 
证 了 闭环 系统 的 稳定 性 。 
(2) 考虑 系统 参数 不 能 够 准确 获知 的 情况 。 
因为 存在 不 确定 项 M、C、G , 式 (9.44) 所 示 的 控制 器 无 法 实现 , 故 需 对 不 确定 项 进一步 分 
析 。 定 义 不 确 定 项 为 五 ”(6,z,), 其 中 6=[ 和 ,6 ] xz 一 [zz zy] 。 注 意 到 9 一 zl 一 
6 十 zy 一 zz? 一 6 十 Bi 8 (el )|:- 0 二 文 4， 可 得 : 
H*:=M(é +za P(t zi) GE tr) + Ce tz ,6 十 Bi(€1 ,4))B(El ,2) 
(9. 45) 
通常 情况 下 ,使 用 径 向 基 函 数 神经 网 络 去 允 近 及“ 的 时 候 , 输 入 z= 二 [&,x,] ERR”, 输入 
数量 为 5n。 为 了 设计 一 个 高 效 、 输 入 数量 更 少 的 神经 网 络 , 假 设 期 望 轨迹 被 完美 地 跟踪 , 即 
€ =[0,0] , 则 HC(0,zx,)= 二 M(xzy)#y 十 C(xy ;yj) 计 十 G(xy)。 令 H(0,x,)=H(x,), 通 过 
将 从 H(E,zx,) 中 移 除 ,神经 网 络 输入 变量 的 数量 减少 到 3n ,这 将 大 大 地 简化 RBF 神经 网 络 
的 结构 。 当 然 ,这 种 简化 也 带 来 了 一 定 的 匹配 误差 6) ,ej (&,x,): 二 有 H(zx,) 一 HH (&,x,)。 根 
据 性 质 4, 互 一 阶 可 导 ,对 匹配 误差 项 s， 应 用 平均 值 定理 ,可 得 : 
le, (ész2) :gclel) el (9. 46) 
其 中 ,g : 及 ”一 了 及 是 一 个 严格 单调 递增 上 且 全 局 可 逆 的 确定 函数 。 
在 此 ,使 用 简化 后 的 一 个 径 向 基 函 数 神经 网 络 逼 近 整 个 不 确定 项 互 (z,) 。 


H(zx,)=$ (zx,)0" —6, (9.47) 
其 中 ,6 为 有 界 的 和 逼近 误差 ,0 定义 为 
bs 一 argmin( sup H(z,) 一 让 (zz 16)1) (9. 48) 
ben, Tren, 


选择 李 雅 普 诺 夫 函 数 , 有 : 
V(t) =Vi()+ 到 5 (9. 49) 
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控制 器 输出 修改 如 下 : 


开 
t= 一 cos (| — (Kpl+Kps)é,—K,sgn(é,)+B'0 (9. 50) 


cn 


其 中 ,增加 的 一 K ,sgn(é&,) 项 用 于 抵消 外 部 干扰 tj ,神经 网 络 的 通 近 误差 6; ,以 及 简化 神经 网 
络 后 的 误差 sy ,增加 系统 的 鲁 棒 性 ; B16 用 于 逼近 不 确定 项 的 集合 。 为 了 满足 下 列 等 式 ， 
Toe, 上 rr-10 一 一 5rAb (9.51) 
其 中 ,A 二 diag(41 ,4,,…,42,) ,注意 到 
DH 十 关于 半生 太公 
由 此 ,设计 更 新 率 如 下 : 


(9. 53) 
8, =—7y,$jé2n — ki@, j=1,2,.,l 
式 (9.49) 对 时 间 求 导 可 得 : 


记 
Vi = Du wn) es K més — 0 A HET K,sgn(é) + — Kt ten) + 


2 
Ee + (Toe, 十 FT- 人 (9. 54) 
a a a 
—0'A6= > 一 1;67 (5 十 0 ) < > (0D + 10) (9.55) 
1 一 1 1 一 1 
参考 式 (9.35) 和 18 ‖ 委 1 861 ,一 1,2(6 一 [6 ,8,]'), 可 得 不 等 式 tle, <<|8,1 lg < 


eleclel),—éeIRKpé, <—k| 2 委 一 下 1 人 2 天王 (CKPp)， 选 择 有 >gC1e1) 
和 上 ,之 64 十 za4 . 一 Mo( 玫 .77 则 可 得 : 


< 
V0) <— 之/ 大 一 tan a 人 2) 一 £2 Kiés— 了 5745 十 二 0 7740 ER 
i=] 


ci 


(C9. 56 
1 *T 关 
其 下 工 Ab ,0 满足 


CR A CA 


p=min (kskase ska da MM 


C9 573 
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式 (9. 56) 不 等 号 两 侧 分 别 乘 以 er ,可 得 下 (eV(1)) 一 src 再 对 等 号 两 侧 积分 可 得 了 < 


A A 考虑 到 状态 变量 和 ,可 得 卫生 V C0) 十， 因此 , 当 
V(0) EL。 ,在 控制 器 即 式 (9.49) 的 控制 下 ,按照 式 (9. 53) 的 更 新 律 更 新 权 值 ,& 将 会 渐 近 收 
敛 于 紧 集 Qi=& | 和 <2V(0)+ 二 |。 

控制 器 中 有 符号 函数 sgn 的 存在 ,为 了 使 控制 器 输出 的 力矩 更 加 平滑 ,将 符号 函数 用 sat 


函数 替代 。 因 此 ,控制 器 最 终 为 : 
| 611 


(EE) 
COS 2 
2& .1 
3 
， > ) 
COS 


Re 


9.2.3 轨迹 跟踪 自 适 应 模块 设计 

轨迹 自 适应 模块 设计 的 目标 是 修改 任务 轨迹 ,并 使 机 械 臂 跟踪 轨迹 时 偏离 任务 轨迹 的 程 
度 与 受到 的 环境 作用 力 之 间 达 到 平衡 。 考 虑 一 般 的 情况 ,机 械 臂 在 三 维 的 任务 空间 中 跟踪 一 
条 从 A 点 到 B 点 的 点 到 点 直线 轨迹 。 跟 踪 过 程 中 受到 环境 力 的 作用 。 


| 
| 


— (Kpi+Kp,)é, — K,sat(é,)+B'0 (9. 58) 


PD 
控制 器 
旋转 任 修正 
| 务 空间 各 轴 轨 迹 
严格 跟踪 
代价 


图 9.13 轨迹 自 适 应 模块 结构 示意 图 


1. 环境 作用 力 的 估计 

由 于 环境 是 未 知 的 , 即 交 互 力 是 未 知 的 ,在 线 调节 技术 要 求 机 械 臂 只 运行 一 次 就 能 够 在 未 
知 环境 中 通过 调节 参考 轨迹 来 平衡 力 与 偏离 任务 轨迹 面积 。 一 种 可 行 的 办 法 是 假定 一 个 环境 
模型 ,然后 在 线 估 计 环 境 模型 的 未 知 参数 ,从 而 估计 交互 力 的 变化 情况 ,以 实现 控制 目标 。 

假设 环境 交互 力 满足 : - 

Fi 一 Fy +K,(X—X¢)+KaX=K,X+KX+K, (9. 59) 

其 中 ,K 7 一 Fu 一 K ,Xo,Fj 和 X 分 别 表示 实际 交互 力 和 当前 末端 执行 器 的 位 置 。 交 互 力 的 
前 馈 部 分 下。、 刚 度 和 矩阵 K, 、 阻 尼 和 矩阵 天。 平衡 点 位 置 X。 都 是 位 置 的 参数 ,都 需要 估计 。 下 
面 给 出 估计 参数 的 更 新 律 ; 
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KR, =R, (0) + diag(y1 ,7y， )| YF -让 
让 .=RR [07 十 diag(ya ,70)| YF —F£) 
0 (9. 60) 
RK, =R ,0) + diag(ys,73)| (F 一 信 ) 
F, =K,Y +R,Y +R, 
其 中 , 廊 ) =[8a, kas] ,R= [kk] ,R=[kyp, kp] Y=[y(t) z(t), F=[f,, 
fj ,Fi 二 [fj,,f1:] 。 机 械 辟 执行 任务 时 ,在 初始 状态 处 于 自由 空间 中 ,没有 受到 外 力作 
用 ,因此 可 以 设置 初 值 为 及 (0) 二 02x1, 民 , (0)==02x1 ,Kj(0)==02x1。 
2. 轨迹 自 适 应 修改 
考虑 一 般 情 况 ,机 械 辟 在 三 维 空间 中 执行 点 到 点 直线 跟踪 运动 ,参考 轨迹 表示 为 X, (上 ) 一 
[zx,(t)y,(t)z,(t)]"。 可 以 在 任务 空间 中 旋转 和 平移 直线 轨迹 ,使 得 站 , (1) 与 x 轴 ( 或 者 y 
轴 、z 轴 ) 重 合 。 
X* (1) =RX,(t)diag(1l,0,0) 
= [x Cy sr dd Ody sw 
=RX, CD) 一。 
= [yw 个)5050 (9. 61) 
其 中 ,X= 一 RX, (i)diag(0,1,1),y,， 和 xz, 是 常数 ,R 是 旋转 矩阵 。 
参考 轨迹 自 适应 模块 通过 修改 期 望 的 任务 轨迹 ,以 减轻 因为 严格 跟踪 任务 轨迹 XX, (z) 而 
受到 的 环境 交互 力 。 为 了 更 直观 地 理解 这 个 过 程 ,这 里 选择 在 任务 空间 中 对 轨迹 进行 修改 ,而 
不 是 在 关节 空间 中 。 为 了 使 机 械 臂 能 够 在 规定 时 间 内 从 起 始点 一 直 向 前 运动 直至 终点 ,旋转 
平移 后 的 轨迹 头 ，(z) 对 应 的 xz 轴 分 量 的 轨迹 应 该 保持 不 变 , 因 此 需要 修改 的 是 y 轴 和 > 轴 的 
参考 轨迹 ,而 轨迹 修改 量 AX(z) 将 沿 着 旋转 平移 后 的 参考 轨迹 修改 获得 .: 


* C2) = (G2) AXG) = [Lz (2) Ay GI FAZED (9. 62) 
最 终 ,期 望 的 参考 轨迹 能 够 通过 再 次 平移 和 旋转 还 原 : 
RR CR 0 (9. 63) 


在 接 下 来 的 分 析 中 ,为 了 简化 描述 ,重新 书写 参考 轨迹 为 匀 , (1) 二 [zx,(t1) 0 0] ,不 失 一 
般 性 地 ,对 于 直线 运动 ,将 参考 轨迹 看 做 只 在 zx 轴 上 运动 ,y 轴 、z 轴 的 参考 轨迹 为 0。 因 此 期 
望 轨迹 的 最 终 描述 形式 为 六 y(t) 二 [x, (7) ,Ay (1),Az(t)]', 式 中 Ay(t) 和 Az(t) 代 表 根 据 交 
互 表现 计算 出 的 轨迹 修改 值 。 

虽然 在 上 述 分 析 中 ,修改 轨迹 时 不 考虑 x 轴 , 但 沿 着 x 轴 的 交互 力 不 能 忽视 ,因此 需要 整 
合 z 轴 的 交互 力 f,,, 到 式 (9. 47) 的 更 新 律 中 : 


| = fi 
[7 SA/ 


其 中 ,了 f,,, 、f,,,， 和 f,,. 为 安装 在 末端 执行 器 的 力 传感器 分 别 沿 z、y 、z 轴 测 量 获得 的 交互 力 。 
交互 力 能 够 通过 式 (9. 60) 和 式 (9. 64) 进 行 估计 获得 : 


出 
F ,一 = 
天 


(9. 64) 


ka,y, thy, +k jy, 


Ra +k,.z, +k jf. 


(9. 65) 
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其 中 y, 、z, 分 别 表示 沿 y 轴 和 > 轴 的 参考 轨迹 ,通常 在 旋转 后 的 轨迹 中 ,y, (+t) 二 y, (zt) 二 0， 
Z(t) = (1)=0。 

由 此 可 以 估算 出 每 个 采样 时 刻 严格 跟踪 任务 轨迹 X, (1) 时 的 代价 值 ,假设 交互 力 和 偏离 
面积 需要 满足 式 (9. 65) ,从 而 保证 两 者 的 平衡 ; 这 将 使 得 式 (9. 8) 的 代价 函数 J 最 小 。 

F- diag(A ,A)(Y,(t) 一 了 ) (9. 66) 

其 中 ,) 是 用 户 定义 的 平衡 下 与 RDA 的 因子 ,Yj (1)==[Ay(t),Az(1)]',Y,=[y,,z,]'。 

因此 ,结合 式 (9. 60) 式 (9. 64) 一 式 (9.66) ,期 望 轨迹 的 表达 式 为 : 

| | 


1 
(ka,y, 4 sy +k ,,) nh 


X,(t) = |X (9. 67) 


和 入 入 
Kacey TR 四 第 sy 


在 自由 空间 中 运动 ( 即 没 有 环境 交互 力作 用 下) = 二 0) 时 , 式 (9. 60) 中 的 参数 将 保持 为 零 。 
式 (9. 60) 和 式 (9. 67) 分 析 可 知 ,Xj (zt) 二 XX, (+), 这 保证 了 在 没有 环境 交互 力作 用 时 ， 该 系统 
能 够 在 轨迹 跟踪 控制 器 的 控制 下 严格 地 跟踪 任务 轨迹 。 


9.2.4 仿真 设计 
1. 机 械 辟 仿真 模型 

下 面 介 绍 使 用 两 连 杆 模型 对 提出 的 在 线 调节 方法 进行 仿真 验证 ,如 图 9. 14 所 示 。 
该 模型 的 动力 学 方程 如 下 : 


ee | Be cd 人 网 网 加 
a 二 = 
Mi M,, 0 cd1 0 ds? p28 72 
(9. 68) 
其 中 ,l= 二 1m,l,==0. 8m,mi 一 0. 5kg,m, 一 0. 5kg,g 一 9. 8N/kg。 
使 用 如 下 简化 缩写 : 
|i 一 Sin(q1 十 qo),c1s 一 cos(ql 十 qy) 
cl 一 cos(ql)，,cy =cos(g,) (9.69) 图 9.14 两 连 杆 机 械 臂 模型 
51 一 Sin(ql),s， 一 Sin(qy) 
式 (9. 68) 中 对 应 元 素 的 表达 如 下 : 
Mi =Cmi +ma)li t+ml? 2m,ll,c, 
Mis =m2l? + m2lil2cs 


Mz =m2l? (9.70) 


c=m,lil,s, 
pi =(m] 十 m2)lics 二 mol,c1 
p2 =m2l2c12 
而 机 械 臂 雅 可 比 矩 阵 表示 如 下 ， 
一 (Ls + Lyd =— 
J(g)= (9.71) 


1C1 十 /2cl2 L2Cc12 
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下 面 将 使 用 上 述 机 械 臂 模型 比较 前 述 所 设计 的 两 个 控制 器 ,对 比 仿真 结果 ,选择 合适 的 轨 
迹 跟 踪 控制 器 ,然后 采用 该 控制 器 对 轨迹 自 适应 模块 进行 仿真 。 

2. 轨迹 跟踪 控制 器 仿真 

轨迹 跟踪 控制 器 的 仿真 主要 验证 控制 器 在 扰动 力 场 存在 时 的 跟踪 性 能 ， 具体 的 跟踪 性 能 
包括 系统 稳定 性 、 输 出 力矩 的 平滑 性 .跟踪 精度 等 。 和 希望 设 计 的 控制 器 能 够 使 机 械 臂 应 用 在 不 
稳定 力 场 中 通过 闭环 控制 使 得 系统 能 够 抵抗 干扰 ,并 较为 准确 地 跟踪 参考 轨迹 。 

使 用 传统 的 PD 控制 器 ,跟踪 直线 轨迹 时 ,受到 发 散 力 场 作用 ; 跟踪 圆 轨迹 时 ,同时 受到 
常量 力 场 .位置 相 关 力 场 和 速度 相关 力 场 的 作用 。 轨 迹 跟 踪 结 果 分 别 如 图 9. 15 和 图 9. 16 
所 示 。 


RS ---- 参 考 轨迹 | 
| 一 一 真实 轨迹 ---- 参 考 轨 还 


3 一 一 真实 轨迹 


E 
让 
0.9 
0.8 
110 710 -0.05 0 0.05 0.10 07 I Ee eA 
二 2 
(a) 直线 轨迹 跟踪 结果 Xm 
(a) 圆 轨 迹 跟踪 结果 
0.02 
E x 
| 0 
_0.02 py 
"i i a i i 
时 间 /s 8 
时 间 /s 
30 30 Ti 
二 4 各 20 
E 一 、 
过 10 E 10 
本 0 71 § 0 
下 -10 oY -10 Tt, 
-20 -20 
-30 -30 
人 用 
时 间 /s 时 间 /s 
(b) 直线 轨迹 跟踪 误差 和 输出 力矩 (b) 圆 轨迹 跟踪 误差 和 输出 力矩 


图 9:15 PD 控制 器 跟踪 直线 轨迹 图 9.16 PD 控制 器 跟踪 圆 轨迹 
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as 


图 9. 15 中 (a) 为 机 械 臂 末端 执行 器 跟踪 结果 ,虚线 表示 参考 轨迹 , 实 线 表示 真实 轨迹 ; 
图 9.16(b) 中 上 面 的 图 表示 跟踪 误差 ,下 面 的 图 表示 控制 器 输出 力矩 大 小 .9 由 图 9. 15 和 
图 9. 16 可 知 ,传统 的 PD 控制 器 在 干扰 存在 的 条 件 下 不 能 够 很 好 地 跟踪 参考 轨迹 ,跟踪 误差 
较 大 ; 且 参 数 设置 值 较 大 ,系统 容易 不 稳定 ,因此 设计 一 个 能 够 在 一 定 程度 上 抵抗 扰动 的 控制 
器 十 分 有 必要 。 下 面 将 对 前 述 两 个 轨迹 跟踪 控制 器 的 性 能 进行 分 析 。 

在 自由 空间 中 跟踪 圆 轨迹 的 逼近 效果 ,如 图 9. 17 所 示 , 其 中 图 9. 17(a) 表 示 逼 近 结 果 , 虚 
线 为 参考 输出 , 实 线 为 实际 输出 ,图 9. 17(b) 表 示 通 近 误 差 。 


~-- -参考 值 
一 一 真实 值 


力矩 Nm) 
© 


中 
时 间 /s 
(a) 圆 轨迹 跟踪 结果 


力矩 /(N:m) 
© 


62 


"me 
时 间 /s 
(b) 圆 轨迹 跟踪 误差 和 输出 力矩 


图 9.17 径 向 基 函 数 允 近 五 。 项 


1) 直线 轨迹 跟踪 

利用 基于 边界 估计 的 滑 模 控制 器 分 别 在 自由 空间 和 发 散 力 场 跟踪 直线 参考 轨迹 的 相关 曲 
线 , 跟 踪 结 果 如 图 9. 18 和 图 9. 19 所 示 。 在 图 9. 18 和 图 9. 19 中 ,分 图 (a) 为 跟踪 结果 (虚线 为 
参考 轨迹 , 实 线 为 实际 轨迹 ); 分 图 (b) 中 上 面 的 图 为 跟踪 误差 ,下 面 的 图 表示 控制 器 输出 力 
和 矩 。 图 9. 20 和 图 9. 21 为 输出 状态 受 限 控制 器 的 仿真 结果 ,分 别 与 图 9.18 和 图 9. 19 相对 应 。 

从 图 9. 18 一 图 9. 21 的 跟踪 表现 看 来 ,无 论 是 在 自由 空间 中 还 是 在 发 散 力 场 中 ,基于 边界 
估计 的 滑 模 控 制 器 和 输出 状态 受 限 自 适应 控制 器 都 能 够 很 好 地 跟踪 直线 轨迹 。 对 比 图 9. 15， 
基于 边界 估计 的 滑 模 控制 器 和 输出 状态 受 限 自 适应 控制 器 都 具有 一 定 程度 的 抗 干 扰 能 力 。 

2) 圆 轨迹 跟踪 

对 圆 轨迹 的 跟踪 分 为 以 下 四 种 情况 : 

。 人 情况 1: 在 自由 空间 中 跟踪 圆 轨迹 ， 

。 情况 2: 在 常量 力 场 中 跟踪 圆 轨迹 ; 

。 情况 3: 在 常量 和 位 置 相关 力 场 中 跟踪 圆 轨迹 ; 

。 情况 4: 在 常量 .位置 相关 力 场 .速度 相关 力 场 中 跟踪 圆 轨迹 。 


@ 在 无 特殊 说 明 时 ,所 有 仿真 结果 图 中 的 分 图 (a)、(b) 中 的 曲线 所 代表 的 意义 均 与 图 9. 15 一 致 。 
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---- 人 参考 轨迹 
一 一 真实 轨迹 


--- -参考 轨迹 
一 一 真实 轨迹 


Yim 


“—0.10 -0.05 0 0.05 0.10 “ -0.10 —0.05 0 0.05 0.10 
X/m X/m 
(a) 直线 轨迹 跟踪 结果 (a) 直线 轨迹 跟踪 结果 


三 = 
水 渣 
张 瑟 
时 间 /s 
30 30 
合 20 合 20 
和 10 2 村 10 2 
际 0 Tl 睦 Tl 
RR -10 六 -一 要 
-20 -20 
eT EE 
时 间 /s 时 间 /s 
(b) 直线 轨迹 跟踪 误差 和 输出 力矩 (b) 直线 轨迹 跟踪 误差 和 输出 力矩 
图 9.18 基于 边界 估计 滑 模 控 制 器 在 图 9. 19 基于 边界 估计 滑 模 控制 器 在 


自由 空间 中 跟踪 直线 轨迹 发 散 力 场 作用 下 跟踪 直线 轨迹 


Yim 


误差 /m 


力矩 /Nm) 


--- -参考 轨 迹 
一 一 真实 轨迹 


1 区 —0.05 0 0.05 0.10 
Xm 
(a) 直线 轨迹 跟踪 结果 


30 
20 
10 2 
0 7 
-10 
-20 
ey Re 
时 间 /s 
(b) 直线 轨迹 跟踪 误差 和 输出 力矩 
图 9.20 输出 状态 受 限 自 适应 控制 器 在 
自由 空间 中 跟踪 直线 轨迹 
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---- 参考 轨迹 
一 一 真实 轨迹 


了 im 


-0.10 -0.05 0 0.05 0.10 
X/m 
(a) 直线 轨迹 跟踪 结果 


1X103 


误差 /m 


力矩 /Nm) 
© 


时 间 /s 
(b) 直线 轨迹 跟踪 误差 和 输出 力矩 


图 9.21 输出 状态 受 限 自 适应 控制 器 在 


发 散 力 场 作 用 下 跟踪 直线 轨迹 


跟踪 结果 分 别 如 图 9. 22 一 图 9. 29 所 示 。 


-03” =02 00 0 0 0a ‘V3 


X/m 
(a) 圆 轨迹 跟踪 结果 


4] 4 6 10 12 14 16 


8 
时 间 /s 


时 间 /s 
(b) 圆 轨 迹 跟踪 误差 和 输出 力矩 
图 9.22 基于 边界 估计 滑 模 控制 器 在 
自由 空间 中 跟踪 圆 轨 迹 


误差 /m 
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本 ---- 参 考 轨 述 
一 一 真实 轨 壕 
V2 

dr 

1.0 

0.9 

0.8 

07 

—0.3 0.2 -0.1 0 0.1 0.2 0.3 

X/m 
(a) 贺 轨 迹 跟踪 结果 


时 间 /s 
(b) 圆 轨迹 跟踪 误差 和 输出 力 抵 
图 9.23 基于 边界 估计 滑 模 控制 器 在 
常量 力 场 作用 下 跟踪 圆 轨迹 


总 体 而 言 , 滑 模 控制 器 的 控制 效果 要 比 状态 输出 受 限 控制 器 要 好 ,跟踪 精度 更 高 。 在 下 文 
将 讨论 的 轨迹 自 适 应 仿真 中 ,将 基于 边界 估计 的 滑 模 控制 器 作为 内 环 轨迹 跟踪 控制 器 ,对 轨迹 


自 适应 环节 进行 仿真 。 
3. 轨迹 自 适应 仿真 


这 里 将 通过 仿真 来 验证 轨迹 自 适应 的 设计 ,假设 没有 环境 交互 作用 时 ,系统 能 够 在 轨迹 
跟踪 控制 器 的 控制 下 严格 地 跟踪 任务 轨迹 。 设 计 要 求 机 械 臂 能 够 在 起 始点 和 终点 之 间 沿 直 
线 移动 3 个 周期 。 选 择 两 个 不 同 的 力 场 ( 力 场 1 和 力 场 2) 进 行 上 述 仿真 ,仿真 结果 如 图 9. 30 


所 示 。 


Yim 


误差 /m 


力矩 /(N:m) 


图 9. 24 


~--- 参 考 轨迹 
一 一 真实 轨迹 


时 间 /s 


2 4 6 3 IN 2 16 


时 间 /s 
(b) 圆 轨迹 跟踪 误差 和 输出 力矩 
基于 边界 估计 滑 模 控制 器 在 常量 、 
位 置 相关 力 场 同时 作用 
下 跟踪 圆 轨迹 
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---- 参考 轨迹 
一 一 真实 轨迹 
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(b) 圆 轨 迹 跟踪 误差 和 输出 力矩 
图 9.25 基于 边界 估计 滑 模 控 制 器 在 常量 、 
位 置 相关 和 速度 相关 力 场 作用 
下 跟踪 圆 轨迹 
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---- 参 考 轨 述 
一 一 真实 轨迹 
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,Oh (b) 圆 轨迹 跟踪 误差 和 输出 力矩 
时 间 /s 
(b) 圆 轨迹 跟踪 误差 和 输出 力矩 
图 9.26 输出 状态 受 限 自 适 应 控制 器 在 图 9.27 输出 状态 受 限 自 适应 控制 器 在 


自由 空间 中 跟踪 圆 轨迹 常量 力 场 作用 下 跟踪 圆 轨迹 


第 9 章 ”其 他 控制 方法 | 有 185 
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(b) 圆 轨迹 跟踪 误差 和 输出 力矩 (b) 圆 轨迹 跟踪 误差 和 输出 力矩 
图 9.28 输出 状态 受 限 自 适应 控制 器 在 常量 、 图 9.29 输出 状态 受 限 自 适应 控制 器 在 常量 、 
位 置 相关 力 场 同时 作用 位 置 相关 和 速度 相关 力 场 作用 


下 跟踪 圆 轨 迹 过 下 跟踪 圆 轨迹 
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第 一 阶段 第 一 阶段 
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(a) 力 场 1 的 仿真 结果 (b) 力 场 2 的 仿真 结果 


图 9. 30 力 场 对 轨迹 自 适应 影响 仿真 结果 


下 面 对 轨迹 自 适应 的 作用 效果 进行 仿真 ,仿真 的 结果 如 图 9. 31 和 图 9. 32 所 示 ,其 中 左 便 
图 表示 的 是 具有 较 低 曲率 的 力 场 , 右 侧 图 表示 具有 较 高 曲率 的 力 场 仿真 效果 图 。 图 中 的 不 同 
线 型 表示 不 同 环境 刚度 下 (环境 刚度 1 和 刚度 2) 的 调节 结果 ,0N/m( 图 中 用 实 线 表示 ) ,100N/m 
(图 中 用 虚线 表示 ), 200N/m( 图 中 用 “o” 标 记 的 实 线 表示 ), 400N/m( 图 中 用 “十 ”标记 的 实 线 
表示 ), 800N/m( 图 中 用 “* ”标记 的 实 线 表示 ) ,点 虚线 表示 力 场 的 轮廓 。 
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X/m 
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Yim Yim 


(a) 不 同 环境 刚度 下 的 参考 轨迹 ， 左 边 为 环境 刚度 1， 右 边 为 环境 刚度 2 


X/m 


030 : 
0 0.05 0.10 .01 0 0.01 0.02 0.03 0.04 
Yim Y/m 


(b) 不 同 环 境 刚 度 下 的 实际 跟踪 轨迹 ， 左 边 为 环境 刚度 1， 右 边 为 环境 刚度 2 
图 9.31 力 场 对 轨迹 自 适应 影响 仿真 结果 一 
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(a) 不 同 环境 刚度 下 的 参考 轨迹 ， 左 边 为 环境 刚度 1， 右 边 为 环境 刚度 2 
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去 去 
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VY/im 7m 
(b) 不 同 环境 刚度 下 的 实际 跟踪 轨迹 ， 左 边 为 环境 刚度 1， 右 边 为 环境 刚度 2 
图 9.32 力 场 对 轨迹 自 适应 影响 仿真 结果 二 


188 去 机 器 人 控制 一 一 运动 学 、 控 制 器 设计 、 人 机 交互 与 应 用 实例 


从 图 9. 32 中 可 以 看 出 , 随 着 环境 刚度 的 增加 ,修改 后 的 期 望 轨迹 逐渐 偏离 原 任务 轨迹 , 当 
环境 刚度 减 小 时 ,期 望 轨迹 向 原 任务 轨迹 靠拢 。 

以 上 仿真 结果 表明 ,轨迹 自 适 应 模块 能 够 在 偏 移 原 参 考 轨 迹 的 同时 ,减少 自身 受到 的 环境 
将 访 。 


9.3 ”机械 医 双 医 协 调 的 视觉 向 服 控制 


本 节 将 介绍 一 种 基于 位 置 的 视觉 伺服 方法 ,使 双 臂 机 器 人 能 够 执行 高 精度 的 双 臂 抓 取 和 
操作 任务 。 

传统 的 单 臂 工业 机 器 人 在 工业 应 用 中 大 量 使 用 ,发 展 成 熟 ,功能 日 趋 稳 定 。 然 而 , 随 着 机 
器 人 作业 场景 的 延伸 ,其 工作 环境 越 来 越 复杂 ,任务 的 复杂 度 和 精细 度 也 越 来 越 高 , 单 辟 机 器 
人 远 远 没有 达到 人 们 的 期 望 。 对 于 比较 复杂 的 工业 装配 任务 .搬运 或 抓 取 较 重 的 物体 、 保 养 和 
维修 结构 复杂 的 工件 等 场景 , 单 臂 机 器 人 都 无 法 胜任 。 单 臂 机 器 人 只 适合 执行 一 些 简 单 、 重 复 
的 任务 ,智能 化 水 平 不 高 。 

通过 双手 协同 互助 ,人 类 能 够 进行 精细 和 复杂 的 操作 。 由 于 双 臂 机 器 人 具有 和 人 类 手臂 
相近 的 机 械 结构 ,同时 受到 人 类 日 常生 活 中 用 双 臂 进行 协同 操作 的 启发 , 双 臂 机 器 人 协调 操作 
应 运 而 生 。 多 个 共同 执行 任务 的 机 器 人 操作 称 为 协同 操作 , 双 臂 协作 分 为 两 类 ,一 类 是 双 臂 并 
行 执行 一 项 任务 ,如 双 臂 夹 持 搬运 物体 ; 另 一 类 是 双 臂 串 行 执行 一 项 任务 ,如 双 臂 倒 水 。 由 此 
可 见 , 双 臂 机 器 人 比 单 臂 机 器 人 具有 更 广 的 使 用 范围 .更 强 的 协作 能 力 和 更 高 的 可 靠 性 ,在 处 
理 能 力 、 承 载 能 力 和 操纵 技能 方面 具有 更 为 突出 的 优势 。 因 此 , 双 臂 机 器 人 不 仅 广泛 应 用 于 工 
业 装 配 、 物 品 搬运 .工业 维修 等 工业 自动 化 领域 ,还 可 以 应 用 于 带 有 双 臂 系统 的 家 庭 服务 机 器 
人 领域 ,完成 单 臂 操 作 难 以 完成 的 任务 。 


9.3.1 青 景 

单 臂 机 器 人 在 工业 应 用 中 大 量 使 用 ,但 智能 化 水 平 不 高 ,只 适合 执行 一 些 简单 .重复 的 任 
务 。 相 比 于 单 臂 机 器 人 , 双 臂 机 器 人 在 处 理 能 力 ,承载 能 力 和 操纵 技能 方面 具有 更 为 突出 的 优 
势 , 因 而 成 为 当今 机 器 人 学 研究 的 热点 。 在 双 臂 协调 控制 的 方法 中 ,视觉 伺服 精度 高 .灵活 性 
好 、 鲁 棒 性 强 , 因 此 机 器 人 学 领域 众多 专家 学 者 纷纷 对 此 展开 研究 。 

双 臂 协调 控制 的 方法 主要 有 主 从 控制 、 混 合 位 置 / 力 控制 和 阻抗 控制 方法 等 。 近 年 来 , 随 着 
机 器 视觉 的 兴起 ,将 双 辟 协调 与 视觉 伺服 控制 相 结合 ,逐渐 成 为 当今 双 辟 协调 控制 方法 的 主流 。 

视觉 伺服 的 任务 是 为 机 器 人 建立 一 个 闭环 控制 回路 ,以 便利 用 视觉 反馈 来 操纵 环境 中 的 


物体 。 一 般 而 言 , 双 臂 机 器 人 的 视觉 伺服 技术 是 由 单 臂 机 器 人 发 展 而 来 的 。 传 统 的 基本 方法 - 


包括 : 基于 图 像 的 视觉 伺服 方法 、 基 于 位 置 的 视觉 伺服 方法 和 混合 视觉 伺服 方法 。 

基于 图 像 的 视觉 伺服 (Image-Based Visual Serving,IBVS) 利 用 所 观察 和 期 望 位 置 的 二 维 图 像 
的 差异 ,在 二 维 平 面 上 完成 特定 特征 的 选择 ,这 种 方法 不 需要 位 姿 的 估计 , 且 运 行 速 度 快 。 基 于 
位 置 的 视觉 伺服 (Position-Based Visual Serving,PBVS) 的 原理 则 是 相对 于 照相 机 而 言 ,利用 所 跟 
踪 物 体 的 观察 位 置 和 目标 位 置 在 三 维 空 间 的 差异 进行 控制 。 因 此 ,可 以 通过 利用 预 捕获 模型 
的 信息 或 深度 信息 计算 物体 笛 卡 儿 空间 中 的 特征 及 其 位 姿 ,从 而 重 构 物体 的 三 维 模型 。 混 合 
视觉 伺服 (又 称 2. 5DVS) 是 IBVS 和 PBVS 两 种 方法 的 结合 ,由 E. Malis 等 人 在 1999 年 提出 。 


9.3.2 视觉 伍 服 控制 
1. 视觉 伺服 基础 
为 了 得 到 摄像 头 坐 标 系 和 机 器 人 坐标 系 的 坐标 转化 关系 ,减少 视觉 目标 定位 和 末端 执行 
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器 定位 之 间 的 误差 ,必须 对 机 器 人 进行 手眼 标定 ,然后 对 目标 物体 识别 与 定位 。 同 时 ,本 节 将 
六 述 视觉 伺服 中 目标 跟踪 的 相应 原理 以 及 相关 的 抓 取 姿态 。 

1) 手眼 标定 

机 器 人 标定 包括 相机 标定 和 手眼 标定 。 相 机 标定 ,主要 是 对 相机 的 内 参 和 外 参 进行 标定 。 
内 参 是 与 相机 自身 特性 相关 的 参数 ,外 参 是 在 世界 坐标 系 中 的 参数 ,如 相机 的 位 置 . 旋 转 方向 
等 。 手 眼 标定 , 则 是 得 到 像素 坐标 系 和 空间 机 械 臂 坐标 系 的 坐标 转化 关系 ,可 以 减少 视觉 目标 
定位 和 末端 执行 器 定位 之 间 的 误差 。 在 双 臂 操作 任务 中 ,两 只 手臂 的 工作 空间 都 必须 在 机 器 
人 的 相机 系统 中 观察 到 。 

2) 目标 识别 与 定位 
”手眼 标定 后 ,就 要 对 目标 物体 进行 识别 与 定位 。 为 了 简化 这 一 问题 ,采用 纯色 物体 ,使 用 
颜色 阔 值 分 割 算法 即 可 将 目标 物体 从 背景 中 分 割 出 来 ,达到 识别 目标 物体 的 目的 ,再 结合 手眼 
标定 得 到 的 转化 矩阵 X, 可 求 得 目标 物体 在 机 器 人 坐标 系 下 的 三 维 坐标 。 

3) 目标 跟踪 

视觉 伺服 方法 通常 跟踪 目标 对 象 的 工具 中 心 点 (Tool Center Point, TCP) 或 特征 ,并 将 
TCP 移动 到 所 需 的 位 置 和 方向 。 视 觉 伺服 的 效果 在 很 大 程度 上 依赖 于 视觉 感知 部 件 计算 出 
来 的 信息 。 为 了 确保 视觉 伺服 的 鲁 棒 执行 ,视觉 伺服 控制 回路 需要 存储 t 时 刻 获得 的 手 和 目 
标的 姿态 。 为 了 处 理 机 械 臂 跟踪 失败 的 情况 ,运动 学 计算 和 视觉 确定 的 机 械 臂 位 置 之 间 的 差 
异 被 计算 出 来 。 在 上 时 刻 , 使 用 机 器 人 的 关节 角度 计算 正 向 运动 学 pf 的 姿态 ,而 且 视 觉 系 统 
中 TCP 位 姿 pwa 的 偏 移 量 可 用 以 下 方法 求 出 : 

Apkin = plin — Pvisual (9.72) 

其 中 , 偏 移 量 Apf 仅 在 机 器 人 当前 机 械 臂 姿势 附近 有 效 。 

在 视觉 感知 失败 的 情况 下 ,可 以 使 用 偏 移 量 Api, 按 以 下 方法 计算 1 十 1 时 刻 姿态 的 估计 
值 pi: 

pi =p uval — Phin 9.73) 

由 于 运动 学 上 和 视觉 上 确定 的 手 位 置 之 间 的 偏 移 Api;, 仅 在 当前 机 械 臂 姿势 附近 有 效 ， 
而 且 当 移动 过 远 时 ,近似值 会 变 得 更 差 ,速度 会 降低 。 如 果 在 特定 时 间 间 隔 内 无 法 再 次 识别 机 
械 臂 , 则 终止 伺服 回路 ,以 避免 不 可 预测 的 危险 情况 发 生 。 

4) 抓 取 姿态 

对 于 目标 物体 的 抓 取 姿 态 如 图 9. 33 所 示 。 为 了 实现 对 于 目标 物体 的 顺利 抓 取 , 特 地 设置 
了 预备 抓 取 姿 态 pxoi4 和 you 作为 抓 取 阶 段 的 过 渡 期 。 以 左手 的 抓 取 姿态 为 例 :, 首先 将 
TCP 移动 到 预备 姿态 p “1… pw ,然后 再 移动 到 最 后 抓 取 姿 态 bye 。 为 了 避免 在 接近 
时 与 目标 对 象 发 生 碰 撞 , 此 过 程 需要 定义 接近 方向 的 动作 wv。 二 p Yo 一 pw4, 来 实现 抓 取 。 
为 此 ,需要 求 得 ; 


Pd De (9. 74) 
eh (9.75) 
在 视觉 伺服 系统 中 ,pon 是 已 知 量 ,可 以 通过 以 下 公式 求 得 on .on ， 
por = CA) Dl (9.76) 
Poldere = (pI ) pelek (9.77) 


2. 抓 取 视 觉 伺 服 
当 对 机 械 臂 的 双 臂 执行 操作 时 ,如 执行 抓 取 任 务 , 需 要 用 1 一 2 个 物体 精确 地 定位 两 只 机 
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图 9.33 抓 取 姿态 


械 臂 。 在 世界 坐标 系 内 ,机 械 臂 与 物体 的 相对 关系 或 相对 位 姿 比 绝对 位 姿 更 为 重要 ,因此 视觉 
上 确定 的 机 械 臂 和 物体 的 相对 姿态 应 该 在 相同 的 图 像 数据 上 进行 。 抓 取 视 觉 伺服 具体 过 程 
如 下 。 
(1) 通过 视觉 系统 确定 目标 物体 在 机 器 人 坐标 系 下 的 位 姿 ,结合 已 知 的 机 械 臂 位 姿 ,得 到 
差分 向 量 Az 王 ze 一 Zhand 来 描述 机 械 臂 和 目标 位 置 之 间 的 差异 。 
(2) 由 于 机 械 臂 与 目标 物体 的 差异 很 大 ,一 步 到 位 抓 取 物体 是 不 现实 的 。 因 此 将 Ax 细 
分 到 抓 取 的 过 程 中 的 每 一 个 预备 姿态 中 ,并 设 定 一 个 阔 值 ,假设 左手 与 右手 抓 取 过 程 中 每 一 
预备 姿态 之 间 的 差分 向 量 分 别 为 Arier 与 Azuaeht, 则 所 期 望 的 关节 速度 g 可 通过 以 下 方程 计 
算得 到 : 
ii a Cad A (9.78) 
人 (9.79) 
其 中 ,a 是 控制 伺服 速度 的 增益 因子 ,J* (g) 是 雅 可 比 矩 阵 Jo ) 的 伪 逆 。 
(3) 如 果 Azien 或 Azuen 低 于 阅 值 , 则 选择 下 一 个 姿势 ,或 者 如 果 机 械 辟 已 经 处 于 最 后 的 
抓 取 位 置 , 则 关闭 机 器 手 的 夹 持 器 ,实现 目标 物体 的 抓 取 。 
在 上 述 视 觉 伺服 方法 中 ,对 机 械 辟 姿态 po。 的 跟 
踪 是 独立 的 ,因此 可 以 从 视觉 观察 到 的 机 械 臂 位置 导 
出 当前 对 象 姿 态 p op: 
on Te To Wie (9. 80) 
机 械 臂 与 目标 物体 之 间 的 转化 关系 工 品 可 以 在 对 
目标 物体 进行 抓 取 的 过 程 中 得 到 , 即 TB = (TY 
(bw) ，。 为 了 操纵 物体 的 姿态 ,可 以 稍微 修改 抓 取 图 9.34 视觉 控制 的 当前 姿态 
的 算法 ,以便 应 用 耦合 或 解 耦 的 双 臂 操作 。 见 图 9. 34。 和 目标 姿态 
3. 基于 视觉 控制 的 双 臂 操作 
研究 平台 为 Baxter 机 器 人 ,如 图 9. 35 所 示 。Baxter 拥有 360" 头 部 超声 波 雷达 ,可 时 刻 检 
查 机 器 人 工作 空间 中 是 否 有 人 ,防止 碰撞 。 左 右 臂 都 是 7 自由 度 的 机 械 臂 ,模拟 人 的 手臂 结 
构 ,灵活 性 高 ; 且 手 部 装 有 摄像 头 及 距离 传感器 ,能 够 适应 变化 的 环境 ,可 “感知 ?异常 现象 并 
引导 部 件 就 位 。 
关于 Baxter 视觉 控制 的 双 臂 操作 流程 图 如 图 9. 36 所 示 。 通 过 Baxter 机 器 人 的 关节 传 感 
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器 得 到 两 个 机 械 辟 的 关节 角度 ,从 而 得 到 双 臂 的 姿态 。 在 视觉 系统 部 分 ,使 用 Bumblebee 摄 
像 头 对 两 个 机 械 辟 的 位 置 和 目标 物体 的 位 姿 进 行 视觉 估计 。 将 所 得 到 的 双 臂 姿态 和 双 臂 位 置 
相 结 合 ,得 到 双 辟 的 位 姿 , 与 之 前 视觉 估计 所 得 到 的 目标 物体 的 位 姿 相 比较 ,得 到 两 个 位 姿 之 
间 的 差分 向 量 Az 。 在 实际 操作 中 ,由 于 双 臂 与 目标 物体 的 位 姿 差 异 很 大 ,将 Az 细 分 到 每 两 
个 相 邻 的 期 望 姿态 之 间 。 差 分 正 向 运动 学 通过 关节 角度 g 和 差分 向 量 Az 求 得 关节 角速度 
6 ,并 将 关节 角速度 0 作为 关节 控制 器 的 输入 ,控制 Baxter 机 器 人 的 关节 运动 ,改变 双 辟 的 次 
态 , 得 到 双 臂 姿态 与 下 一 姿态 的 差分 向 量 。 不 断 重复 上 述 过 程 , 直 至 最 后 双 臂 达到 最 终 姿态 。 


图 9. 36 ~ 双 臂 操作 流程 图 


4. 基于 示 教 的 双 臂 操作 


在 视觉 伺服 系统 下 , 当 双 臂 成 功 抓 取 目 标 物 体 后 ,采用 示 教 的 方式 进行 倒 水 的 双 臂 操作 。 
与 传统 的 机 器 人 不 同 的 是 ,Baxter 可 以 通过 示 教 完成 一 系列 简单 的 任务 。 示 教 一 般 分 为 两 部 
分 一 一 教学 部 分 和 复 现 部 分 。 在 教学 部 分 ,人 为 地 移动 Baxter 的 机 械 辟 到 达 不 同 的 位 姿 或 改 
变 夹 持 器 的 状态 ,计算 机 需要 记录 这 一 系列 的 位 姿 以 及 夹 持 器 的 状态 。 在 复 现 部 分 ,计算 机 根 
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据 所 记录 的 位 姿 以 及 夹 持 器 的 状态 ,操控 Baxter 机 器 人 复 现 这 一 系列 位 姿 ,完成 相关 任务 。 
而 且 Baxter 的 手臂 上 有 额外 的 刻度 盘 、 按 钮 和 控件 ,以 获得 更 高 的 精度 和 功能 。 对 于 大 多 数 
其 他 工业 机 器 人 而 言 ,都 需要 专业 的 计算 机 程序 员 对 它们 进行 编码 。 然 而 ,具有 示 教 功能 
的 Baxter 可 以 在 几 分 钟 内 由 非 熟练 工人 完成 编程 。 倒 水 双 臂 操作 的 复 现 流程 图 如 图 9. 37 
所 示 。 


左手 抓 取水 杯 左手 到 达 预 备 | 
物体 成 功 倒 水 位 姿 1 . 
右手 抓 取 瓶 子 左手 到 达 预 备 | .. 
物体 成 功 倒 水 位 姿 1 


左手 到 达 预 备 
倒 水 位 姿 m 
左手 到 达 预 备 
倒 水 位 姿 m 


左手 固定 
右手 到 达 
倒 水 位 姿 1 


左手 固定 
右手 到 达 
倒 水 位 姿 n 


图 9.37 倒 水 双 臂 操作 的 复 现 流程 图 


9.3.3 实验 

1. 视觉 伺服 下 的 抓 取 

为 了 保证 在 视觉 系统 下 实现 抓 取 ,在 抓 取 
的 过 程 中 加 入 了 一 些 过 渡 位 姿 。 在 抓 取 的 过 程 
中 ,Bumblebee 双 目 摄像 头 检测 到 目标 物体 , 识 
别 后 将 其 坐标 转化 为 机 器 人 坐标 系 下 三 维 坐 
标 , 使 用 MATLAB 储存 该 坐标 后 ,通过 ROS 
机 器 人 控制 系统 控制 Baxter 机 器 人 末端 执行 器 
到 达 目 标 物体 的 相应 位 置 ,从 而 实现 抓 取 , 如 
图 9. 38 所 示 。 

2. 视觉 伺服 下 的 跟踪 

利用 Bumblebee 双 目 摄像 头 实 时 连续 拍 
照 ,不 断 地 获得 目标 物体 在 机 器 人 坐标 系 下 的 WE 
三 维 坐标 。 当 人 为 改变 目标 物体 的 坐标 时 ,图 图 9.38 _ 抓 取 
像 中 目标 物体 的 位 置 和 实际 中 目标 物体 的 位 置 
就 会 存在 误差 。 设 定 误 差 冰 值 为 0.1, 即 如 果 两 个 三 维 坐标 的 误差 之 和 达到 0. 1, 可 以 认为 
目标 物体 的 坐标 发 生 了 变化 ,需要 把 目标 物体 新 的 坐标 通过 MATLAB 传达 到 ROS 机 器 人 
控制 系统 ,从 而 控制 Baxter 末端 执行 器 到 达 新 的 位 置 ,实现 视觉 伺服 下 的 跟踪 ,如 图 9. 39 
所 示 。 

3. 示 教 倒 水 实验 

当 双 壁 成 功 抓 取 目 标 物体 后 ,可 以 对 Baxter 机 器 人 采用 示 教 的 方式 进行 倒 水 的 双 臂 操 


第 9 章 ”其 他 控制 方法 | 加 193 


(b) 右手 跟踪 
图 9.39 视觉 跟踪 
作 。 为 了 避免 机 械 臂 的 正 向 运动 学 中 的 奇异 点 ,在 倒 水 过 程 中 设置 了 一 系列 预备 倒 水 位 姿 和 


倒 水 位 姿 ,并 在 教学 时 提前 将 预备 倒 水 姿势 和 倒 水 姿势 记录 到 计算 机 。 双 臂 倒 水 的 操作 如 
图 9. 40 所 示 。 


图 9.40 示 教 倒 水 的 操作 


9.4 基于 神经 网 络 的 未 知 机 械 璧 导 纳 控制 


本 节 将 介绍 一 种 基于 神经 网 络 的 无 力 传感器 机 械 臂 与 人 体 手 臂 物 理 交 互 的 控制 方案 。 该 
方案 采用 几何 矢量 法 和 Kinect 传感器 生成 轨迹 。 为 了 适应 外 部 环境 的 力矩 ,提出 了 一 种 基于 
观测 器 的 关节 空间 无 传感器 导 纳 控制 方法 ,用 于 估计 操作 者 施加 的 外 部 力矩 。 针 对 不 确定 机 
械 辟 的 跟踪 问题 ,设计 了 一 种 基于 径 向 基 函 数 神 经 网 络 (Radial Basis Function Neural 
Network,RBFNN) 的 自 适 应 控制 器 ,采用 RBFNN 对 系统 的 不 确定 性 进行 补偿 。 为 了 达到 规 
定 的 跟踪 精度 ,将 误差 变换 算法 集成 到 控制 器 中 ,并 利用 李 雅 普 诺 夫 函 数 分 析 控 制 系统 的 稳定 
性 。 通 过 Baxter 机 器 人 实验 ,验证 了 该 控制 方案 的 有 效 性 和 正确 性 。 

在 过 去 的 几 十 年 里 ,机 器 人 在 工业 、 服 务 和 医疗 等 各 个 领域 得 到 了 广泛 的 应 用 。 机 器 人 不 
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仅 可 以 提高 生活 质量 ,还 可 以 提高 工作 效率 ,完成 人 们 在 一 定 条 件 下 无 法 完成 的 工作 。 但 是 传 
统 的 机 器 人 操作 方法 往往 需要 使 用 外 部 设备 和 软件 ,给 操作 人 员 带 来 不 便 , 降 低 了 生产 效率 。 
另 一 种 让 机 器 人 与 人 类 直接 互动 的 方法 是 让 机 器 人 学 习 人 类 的 技能 。 

传统 的 运动 捕捉 方法 要 求 操 作者 将 传感器 固定 在 人 体 的 各 个 关节 上 ， 捕 扣 人体 的 运动 ， 但 
这 会 带 来 很 多 不 便 。 近 年 来 ,基于 视觉 的 运动 捕捉 方案 为 实现 这 一 目标 提供 了 另 一 种 思路 。 
由 于 其 方便 \ 准 确 等 优点 ,这 种 基于 视觉 的 方案 在 机 器 人 中 得 到 了 广泛 的 应 用 。 该 方案 采用 摄 
像 机 捕捉 人 体 运 动 ,避免 了 操作 人 员 佩 戴 大 量 可 穿戴 设备 。 本 节 使 用 的 运动 捕捉 相机 是 微软 
公司 开发 的 Kinect(2.0 版 )。 通 过 在 Kinect 传感器 中 能 入 RGB 摄像 头 和 深度 传感器 ,得 到 人 
体 各 关节 的 三 维 坐 标 ( 关 于 Kinect 传感器 的 详细 介绍 请 参考 10. 3. 3 节 )。 在 此 基础 上 ,利用 
几何 向 量 的 方法 ,计算 出 人 体 手臂 的 各 个 关节 角度 ,并 生成 所 需 的 轨迹 。 

在 实际 的 遥 操 作 控制 系统 中 ,机 器 人 可 能 会 遇 到 来 自 外 界 环境 的 外 力 。 能 让 机 器 人 的 行 
为 与 环境 力 相 适应 的 一 种 控制 方法 是 阻抗 控制 。 “阻抗 ”的 概念 是 由 Hogan 提出 的 ,并 用 于 人 
与 机 器 人 的 物理 交互 中 。 目 前 ,该 方法 已 成 为 机 器 人 领域 的 经 典 控制 方法 。 阻 抗 控 制 方法 的 
核心 思想 是 将 广义 位 置 和 速度 映射 到 广义 力 上 。 如 果 控 制 一 个 机 构 的 阻抗 ,就 控制 了 外 界 运 
动 所 产生 的 阻力 。 从 实用 的 角度 出 发 ,通常 把 机 器 人 的 行为 看 作 是 末端 执行 器 的 姿态 , 它 是 在 
直角 坐标 系 下 定义 的 。 第 卡 儿 坐标 系 下 的 位 置 和 速度 是 控制 器 的 输入 ,电机 力矩 为 输出 。 另 
一 种 方法 是 导 纳 控制 , 它 在 工业 机 器 人 中 得 到 了 广泛 的 应 用 。 导 纳 是 阻抗 的 倒数 , 导 纳 控制 定 
义 了 由 力 输 入 引起 的 运动 。 各 关节 的 外 力作 为 导 纳 控制 结构 的 输入 ,产生 新 的 运动 。 因 此 ,用 
于 接收 外 力 的 力 传 感 器 在 导 纳 控制 系统 中 得 到 了 广泛 的 应 用 。 测 量 外 力 的 一 般 思路 是 将 力 传 
感 器 固定 在 机 械 臂 上 ,而 这 些 传感器 一 般 比 较 昂 贵 。 由 于 这 些 原因 ,估算 外 力 的 相关 技术 受到 
了 极 大 的 关注 ,相关 学 者 也 提出 了 很 多 方案 。 例 如 ,有 学 者 提出 了 机 器 人 应 用 的 早期 估计 方 
法 ; 有 学 者 分 析 了 基于 电机 力矩 .关节 角 和 速度 的 干扰 观测 器 方法 ; 也 有 学 者 提出 了 一 种 基 
于 广义 动量 的 无 传感器 机 器 人 碰撞 检测 方法 。 

在 导 纳 控制 下 , 当 机 器 人 末端 检测 到 外 力 时 ,机 器 人 的 参考 轨迹 将 会 改变 ,控制 器 将 跟踪 
修正 后 的 参考 轨迹 。 在 遥 操 作 控制 系统 中 ,机 械 臂 的 跟踪 精度 至 关 重 要 。 无 模型 控制 和 基于 
模型 控制 是 机 械 臂 的 两 种 控制 方法 。 与 无 模型 控制 方法 相 比 ,基于 模型 的 控制 方法 通常 能 够 
取得 比较 满意 的 控制 效果 。 但 是 该 方法 过 于 依赖 模型 的 精确 度 , 而 实际 系统 中 往往 存在 不 确 
定性 ,很 难得 到 机 器 人 的 精确 模型 。 如 何 处 理 不 确定 性 已 成 为 控制 设计 中 的 核心 问题 。 一 般 
而 言 ,最 常用 的 方法 之 一 是 自 适应 控制 ,该 方法 不 需要 系统 参数 的 先 验 信息 。 

在 实际 应 用 中 ,要 求 同 时 考虑 暂 态 性 能 和 稳 态 性 能 。 然 而 ,大 多 数 通用 的 自 适应 控制 方法 只 
保证 系统 的 稳定 能 ,难以 解决 暂 态 性 问题 。 本 节 使 用 误差 转换 技术 将 跟踪 误差 控制 在 期 望 的 水 平 。 

9.4.1 力 观 测 器 

1. 机 器 人 动力 学 建 模 

一 个 连 杆 的 机 械 辟 动力 学 为 : 

M(g)g+C(g I +G(g) tro =rt (9. 81) 
式 中 ,qER"” 为 关节 角 位 移 向 量 ; r €E R” 是 控制 力矩 输入 ; M(g)、Clq,9) 和 G(g) 分 别 表 示 
对 称 惯性 矩阵 离心力 和 哥 氏 力矩 阵 .重力 项 ; revER” 是 连 杆 中 的 外 加 干扰 。 
机 械 臂 有 如 下 动力 学 特性 : 
(1) M(o) 一 2C(q,0) 是 一 个 斜 对 称 矩 阵 ,对 于 zxER” ,满足 
rz!'(M(g)—2C(g,9))z =0 (9. 82) 
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(2) M (gq) 为 对 称 正定 矩阵 , 3m ;>0,my>0,zER" 满足 如 下 不 等 式 : 
mi zl Memlzl’ (9. 83) 
(3) 存在 一 个 参数 矩阵 使 得 M(a)、.C(Ca,a) 和 
G(g) 满 足 如 下 线性 关系 : 
M(qg)9 二 Cod)o 十 Mo) ro = (gqg,9,9,0)P 
(9. 84) 
其 中 ,@ (g ,9 ,9,o) 是 关节 变量 函数 的 回归 和 矩阵 ,P 
是 描述 机 械 臂 质量 特性 的 未 知 定常 参数 向 量 。 
两 连 杆 机 械 臂 结构 如 图 9. 41 所 示 。 图 中 ,m， 
-为 第 i 个 连 杆 的 质量 ,i; 为 第 i 个 连 杆 的 长 度 ,i 二 1， 
2; !. 表示 连接 第 i 个 连 杆 和 第 i 一 1 个 连 杆 的 质心 
距离 ,T; 是 第 ; 个 连 杆 的 转动 惯量 。 
根据 拉 格 朗 日 - 欧 拉 方程 对 机 械 臂 进行 动力 学 
建 模 ,可 得 M(g)、C(g,9) 和 G(g) 表 达 式 分 别 . 
如 下 ， 图 9.41 两 连 杆 机 械 臂 结构 
ae ee | 
G(g)= 
力 45) gecoskgl 十 Gazyh 


p(lD+t p22p( (aco0s(gd) Ip(2) pS)Cos(qs» 
<M(g)= (9. 85) 
2) p33Yeos(g;) (2 
—p(W08in(qay "rp(3)(d -tossin(g;) 
Cl(g,9)= 
p(3)g1sin(g,) 0 


其 中 ,sg 为 重力 加 速度 , 且 : 
Pp(D) =mile tm2li+t+ 1 
p(2) =m,lis 十 T， 
pl3) =moll,, 
p(4) =mils mol 
p(5)=m,l., 
2. 力 观 测 器 设计 
本 节 将 介绍 基于 广义 动量 的 力 观 测 器 设计 。n 连 杆 的 机 器 人 的 动力 学 如 式 (9.81) 所 示 。 
定义 广义 动量 为 : 


p=M(g)g (9. 86) 
对 式 (9. 86) 求 关于 时 间 +t 的 导数 ,可 得 : 
b=M(g)9 + M(g)g (9. 87) 
将 式 (9. 87) 代 入 式 (9. 81) 中 得 : 
、 户 =M(o) 十 rz 一 CCq,o)0 一 GGCa) 一 re (9. 88) 
化 简 式 (9. 88) ,可 得 : 
b=C'(g,9) rt—G(g) 一 rs (9. 89) 


可 以 看 出 ,基于 广义 动量 的 观测 器 方法 , 即 式 (9. 89) 不 涉及 关节 角 加 速度 94 。 外 力矩 的 模 
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型 为 : 
Pet = Fs (9. 90) 
其 中 ,w, ~~N (0,Q, ) 表 示 不 确定 项 。 和 矩阵 A, 通常 定义 为 A, 二 0, X0,。 负 定 A,. 可 以 减少 扰 

动 带 来 的 估计 偏差 。 则 式 (9. 89) 可 以 表示 为 : 

De (9. 91) 

其 中 : 
u=C!'(g,9)+rt—G(g) (9. 92) 

用 状态 空间 的 形式 重新 表述 上 述 方程 : 


0， 一 0; 
| Fs | 机 | 十 | & 十 
ext 0， As T ext 0 WL (9.93) 
se 


其 中 , 是 测量 噪声 vu~N (0,R,), 容 易 证 明 ,该 系统 是 可 观测 的 。 由 于 关节 角 q 、 关 节 角 速度 
4 是 可 以 测量 的 ,所 以 式 (9. 86) 中 定义 的 广义 动量 也 是 可 测 的 。 


设计 如 下 状态 观测 器 
么 一 4 十 Box 十 工 (y 一 9) 
(9. 94) 
y= 二 Cc2 
0， = Ls 
其 中 ,ce 一 人 吉 )'Be=(0) ,ce= 1, 0 z=Lpren]. 
观测 器 的 增益 矩阵 计算 如 下 : 
天 三 PCeRE: (9. 95) 
其 中 ,和 矩阵 P 由 代数 Riccati 方程 计算 得 到 
AcP+PAL— PCLReCeP+Qc=0 (9. 96) 


式 中 ,0Oc 一 diag[(0,Q。)] 为 状态 的 不 确定 项 。 
力 观 测 器 示意 图 如 图 9. 42 所 示 ,输出 y 为 Ccz( 刀 和 Cece() 的 差 值 ,以 增益 矩阵 工 作为 
校正 项 。 如 果 增 益 矩 阵 工 合适 , 力 观测 器 的 估计 值 会 逼近 实际 值 。 根 据 线性 系统 理论 ,有 ， 
人 一 (4 一 LCc) 人 十 Box 十 工 y (9. 97) 
定义 观测 误差: e 王 工 一 全 , 则 
E 一 工 一 人 =4z 十 Bu 一 (A 一 Ce) 人 一 Bo 一 Ly=(4c 一 LCc)e (9.98) 


图 9.42 力 观测 器 示意 图 
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由 式 (9. 98) 可 知 , 观 测 误差 由 (A 一 LCc ) 的 极点 配置 决定 , 当 (Ac ,Cc ) 可 观测 情况 下 可 以 任 
意 配 置 极 点 。 由 Ac 和 Ce 的 定义 可 知 , (4c ,Ce ) 是 可 观测 的 。 而 (4 一 Ce ) 的 极点 选择 决 
定 了 观测 器 的 估计 值 是 否 会 逼近 真实 值 。 

9.4.2 控制 器 设计 

控制 器 的 设计 是 为 了 保证 机 器 人 在 Kinect 产生 的 关节 空间 中 能 够 按照 期 望 的 轨迹 运动 。 

1. 导 纳 控制 

机 械 臂 的 运动 学 为 : 

; X(t) =xkr(g) 《9. 99) 

其 中 ,ck(。) zt 和 qoER"。<(，) 为 机 器 人 正 向 运动 学 函数 。 

对 式 (9.99) 求 导 , 可 得 .: 


Z(t)=J(g)g (9. 100) 

en 为 雅 可 比 矩 阵 。 对 之 人) 求 导 得 
(1) = (g++ (gg (0; 101) 

定义 币 卡 儿 空 间 坐 标 系 下 机 械 辟 导 纳 模型 为 .; 
fext =f (r,s Ta) (9. 102) 


其 中 ,zx, 为 修正 参考 轨迹 ,x 为 初始 参考 轨迹 。f(，) 是 导 纳 模型 函数 。 
将 式 (9.99) 一 式 (9.101) 代 入 二 阶 导 纳 模型 ,可 得 ; 
MUJ(Co)Ca —34) + Myj (gq) +CT gd, 一 0) 二 GCCcGCdo 一 c(Cqy)) 一 一 Jo)res 
(9. 103) 
其 中 ,Mu .Cy， 和 Gy 为 常数 矩阵 ,f ox 二 J (gq)tow。 
简化 的 导 纳 模型 为 : 
Gu (gq; 一 gay) 一 rext (9. 104) 
由 以 上 导 纳 模型 可 以 知 ,机 械 辟 不 受 外 力 时 , 即 r= 二 0,g, 二 94 ,控制 器 将 跟踪 初始 参考 
轨迹 gj, 。 当 机 械 臂 受 外 力 时 ,控制 器 将 跟踪 由 导 纳 模型 解 出 的 修正 的 参考 轨迹 q， 
2. 控制 器 设计 
这 里 使 用 误差 转换 方法 保证 系统 暂 态 控制 效果 ， EE 进行 
估计 ,以 保证 系统 的 稳定 。 
1) 误差 转换 
定义 机 械 辟 的 跟踪 误差 为 : | 
| = yg 
(C9. 105) 


le =G—v 


v” 将 在 后 面 定义 。 控 制 器 的 目标 是 使 实际 关节 轨迹 g 有 效 地 跟踪 期 望 关节 轨迹 go。 首先 ， 
定义 一 个 光滑 且 有 界 的 约束 函数 

Pp() = (po = pea)e * pos (9. 106) 
其 中 ,oo .ps 和 均 为 正 实数 。 为 了 保证 跟踪 误差 能 够 满足 瞬 态 性 能 ,引入 以 下 转换 函数 . 


eslt) =p(R, (P 人 


(1) 
EXpKi 一 5 3 
要 一 -一 一 ， 当 e(0) 盖 0 时 (9., 107》 
1 二 exp(t) 
R,;(t)= 
oexp(t)—1 


当 ey(0)0 时 


1 十 exp (1) 
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其 中 ,R;(，) 是 P;(，) 的 反 函 数 ,P,(。) 为 ; 
Ee 当 e,(0) 之 0 时 


(9.108) 


ln 7 当 ew (0) 委 0 时 


二 
式 中 ,o 为 正 实数 。 根 据 R;(。) ,轨迹 跟踪 误差 es01) 为 : 一 op (1)<<e, (1)<p(t),e, (1)>0; 
一 p(t)<e, (1)<op(t) ,es(t) 研 0。 因 此 , 超 调 量 A 在 暂 态 阶段 有 界 : 
一 op(0) 之 A 过 pl(0)， 当 es(0)>0 时 


《9. 109) 


一 p07 过 A 这 ey(0) 委 0 时 
跟踪 误差 的 幅 值 不 超过 Lo- ,op ] 的 最 大 值 ,最 大 超 调 量 不 超过 [ooo; ,一 cou ]。 调 整 时 间 
是 系统 的 响应 达到 输入 的 100% 士 5% 时 所 用 的 时 间 , 控 制 系统 的 调整 时 间 小 于 (max(1,o)/ 
p)ln《po 一 pw/1.05p)。 因 此 ,可 以 通过 设置 适当 的 参数 来 控制 系统 的 暂 态 和 稳 态 性 能 。 
根据 式 (9. 108) ,定义 : 


[| 
m0 =P, (0 ) (9. 110) 
pe 
则 期 望 的 关节 速度 vs (z) 为 : 
of (0) = kes Ct) (0 + he Ce) | (9.111) 
其 中 ,k; 为 正 的 常数 。 
1 > 
定义 李 雅 普 诺 夫 函数 Vi; 一 7 《1) w(t) ,对 其 求 导 可 得 : 
7CD) 己 (7CG))esCz) a 
Lt 2 (9. 112) 
其 中 : 
P(nC()) =diag (Pi (Ri CT GD))…P (R, CT (1)))) 
(COLTS 


vs 二 [a ,Ud ud] 
2) 基于 RBF 神经 网 络 的 关节 速度 控制 
对 关节 速度 进行 控制 的 目的 是 让 速度 误差 尽 可 能 地 小 。7w,; (1) 对 时 间 求 导 , 并 代入 
式 (9. 81) ,可 得 : 
M(o)e 二 Co)ed 二 Go) 二 rz =r +MY 二 CC)u4 (9. 114) 
Plnlt) 
Mo 000 
p(t) 
设计 如 下 控制 力矩 : 
t=—kye, — M(q)v  — Cg,9)v 二 Go) 十 (9. 115) 
式 中 ,MCo)、C(o,a) 和 Ca) 均 为 神经 网 络 的 估计 项 。 由 函数 通 近 技术 (Function 
Approximation Technique,FAT) ,可 得 : 
王 全 六 SCz) er (zx) 
C(a,o)uov =Wi Sc(z) ec (z) (9. 116) 


G’'(g) =Weé''So'(z) +el (z) 


o 
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其 中 ,WAT .WiT 和 We 表示 理想 的 权重 矩阵 。 利 用 RBF 神经 网 络 , 则 M (g).C(g,9) 和 
G'(q) 的 估计 值 为 ; , 
| =WISwm (z) 
Ca,a)u4d =WLSc (z) (9.117) 


| CG’'(g) =WL,So (z) 
则 动力 学 方程 可 写成 如 下 形式 : 


P(n(t)) 
M(g)2 +C(gsd)e, 十 到 = Mg) 一 疹 (0 证 (Cg 人 一 
Clg,9))v — (GC(g) —G(g)) 十 (rs 一 人) 
(9. 118) 
式 中 , 星 (o) Co, 各 (Co) 均 为 估计 和 矩阵。 
en ! ee 
考虑 李 雅 普 诺 夫 函 数 V， gel) MU)e, (z) ,其 微分 形式 如 下 : 
ee Ty 人 
V, = 7 esM(g)e, +e,M(g)e, 
= 一 让 e524 oe WS(2) Eee ec) 一 ED (9. 119) 
其 中 ,W = Wi 十 We 一 WO ,WW 二 W 一 W,e(z)=ey (z)+ec(z) eo (z) ,ep =rton ton 
通过 如 下 表达 SP 笃 网 络 权重 矩阵 全: 
es (9. 120) 
eT Py 
其 中 ,@(， ) 和 7 均 为 常数 矩阵 。 
3. 稳定 性 分 析 
ed 


V=Vi 十 Vs 十 tr(WHOH Ww) 十 广 >tr(WLOc We) + FWieo Wo ) Co, 21) 
对 式 (9. 121) 求 导 , 可 得 ; 
V=—kin (CPO)) nt) 一 le, 2 一 ezey 十 
ele(z)+elW!S(z) tr(WienW,) 一 
tr( 太 TIW。 ) 一 tr (WI W, ) (9. 122) 
由 上 式 推导 出 : 
V <—Riy PO yt) 一 Ra le, 1 一 eleyr 十 


1 1 
eels) pr | 2 ymtr(WhWw) 十 


yetr WEIWe) + Yetr (WIW,) (9. 123) 
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根据 已 (7; (7)) 的 定义 ,有 : 


7 (PO)) nt) 之 el (9. 124) 
由 Young 不 等 式 ,可 知 以 下 不 等 式 成 立 : 
Colt a ga Pe 1 ee | > . 
WW’ 一 商 ) <— 1WI + lw |=etey SFle,l’ + le l’ (9.125) 
A 
也 vU 2 
因此 ,根据 式 (9. 124) 可 推导 出 : 
人 1 了 1 ki] 5 2 
VST le 性 一 元 下 er 村 一 2 二 7 一 人 一 于 ) le, 12 十 
1 人 i A 二 
FYtr (Wa Wa 二 We We +Weé Weé 2 
(9. 126) 


1 A oe 
FYtr Wu Wu +WEWe +WLW,) 


1 
选取 ks 了 闻 , 若 式 (9. 126) 满 足 不 等 式 : 


1 | i i i k 
二 |‖e(z) | ?+oytr Wi We +We We +WeE We) 和 2 一 
4 2 i= 


5 1 RAR 
(一 了 ) le,l? + lerl’ + srr Wim HWEWe +WIWo) (9. 127) 


可 得 V 过 0, 即 系统 满足 UUB 超 定 甸 件 。 
综 上 可 得 , w(t) | 、| 僻 | 和 由 e, 有 界 ,e。 也 是 有 界 的 。 至 此 ,系统 稳定 性 分 析 完毕 


9.4.3 仿真 验证 
实验 基于 Baxter 双 臂 机 器 人 (如 图 9. 43 所 示 ) 进 行 仿真 验证 。 


图 9. 43 ” Baxter 双 臂 机 器 人 


1. 神经 网 络 跟踪 性 能 仿真 
本 实验 主要 验证 神经 自 适 应 控制 器 的 有 效 性 。 通 过 Kinect 传感器 采集 操作 者 肘 曲 角 和 


肘 转 角 的 运动 轨迹 。 数 据 采集 过 程 中 ,要 求 操作 者 保持 身体 稳定 ,操作 者 手臂 周期 性 地 平稳 运 
动 , 如 图 9. 44 所 示 。 
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一 一 肘 曲 角 


关节 角度 /rad 
[Re 
© 


0 -20 40 60 80 100 120 140 160 180 200 
帧 


图 9. 44 操作 者 从 起 始 位 置 到 目标 位 置 以 低速 旋转 手臂 


仿真 时 ,输入 参考 轨迹 为 如 图 9. 44 所 示 的 轨迹 。 仿 真 时 间 1, 二 20s, 关 节 角 的 初始 值 
gi 二 0rad,qs 二 一 lrad; 关节 角速度 的 初始 值 为 91 二 0rad/s,gs 二 0rad/s。 为 保证 跟踪 性 能 ， 
控制 器 参数 设置 为 po 二 0.2,p, 二 0.03,0= 二 5, 控 制 增益 二 [12,1]' ,ks 二 [15,1]' ,神经 网 络 
权重 矩阵 的 初始 值 为 僻 5 (0)=0ER”**X' ,WEL(0) 二 0ER”X! ,Wt,(0) 二 0ER"*X!， 

这 里 对 带 误差 转换 的 控制 器 跟踪 性 能 进行 验证 ,并 与 FAT 控制 器 和 变 刚度 控制 器 的 跟 
踪 结 果 进 行 对 比 ,实验 结果 如 图 9.45 和 图 9. 46 所 示 。 如 图 9.45(a)、(b) 和 图 9. 46(a)、(b) 所 
示 , 无 论 是 在 瞬 态 还 是 稳 态 阶段 ,实际 轨迹 都 能 很 好 地 跟随 所 期 望 的 轨迹 ,跟踪 误差 都 能 收敛 
到 式 (9. 106) 中 所 定义 的 规定 边界 。 两 关节 神经 网 络 权 值 范 数 的 变化 如 图 9. 47 所 示 ,控制 器 
的 输入 力矩 如 图 9. 48 所 示 ,可 以 看 出 ,控制 器 的 输入 有 界 。 图 ,9. 45(c)、(d) 、(f) 为 三 种 控制 器 
的 跟踪 性 能 ,对 应 的 跟踪 误差 如 图 9.46(c)、(d)、(f) 所 示 。 从 图 中 可 以 看 出 ,在 没有 有 瞬 态 约束 
控制 的 情况 下 ,跟踪 误差 超出 了 规定 边界 ,稳定 相位 的 误差 相对 较 大 。 实 验 结果 表明 , 带 误差 
转换 的 神经 网 络 自 适应 控制 器 在 暂 态 阶 段 和 稳定 阶段 均 能 保证 跟踪 误差 不 超过 规定 的 界限 ， 
且 控 制 效果 优 于 另外 两 种 控制 器 。 


1.0 一 期 望 轨迹 
--- 实际 轨迹 


3 0.5 
拒 0 
状 -05 
0 5 10 15 20 
时 间 /s 
(a) 带 误差 转换 的 控制 器 对 关节 1 轨迹 的 跟踪 结果 
3 一 期 望 轨迹 
.--- 实际 轨迹 
1.0 
S 
护 
六 0.5 
00 5 10 15 20 
时 间 /s 


(b) 带 误 差 转 换 的 控制 器 对 关节 2 轨迹 的 跟踪 结果 
图 9.45 三 种 控制 器 对 两 关节 角 轨 迹 的 跟踪 结果 
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:0 一 期 望 轨迹 
--- 实 际 轨迹 


-0 
污 0 
六 -05 
“10 5 10 15 20 
时 间 /s 
(c) FAT 控 制 器 对 关节 1 轨迹 的 跟踪 结果 
1 一 期 望 轨迹 
--- 实 际 轨迹 
遇 ，10 
中 
水 0.5 
0 5 10 |] 0 
时 间 /s 
(d) FAT 控 制 器 对 关节 2 轨迹 的 跟踪 结果 
Lo 3 一 期 望 轨迹 
-~… 实 际 轨迹 
有 0.5 (a 
已 小 宙 
六 -05 
0 5 10 15 20 
时 间 /s 
(e) 变 刚度 控制 器 对 关节 1 轨迹 的 跟踪 结 
> 一 期 望 轨迹 
--- 实际 轨迹 
时 1.0 
S 
护 
六 0.5 
00 5 10 15 20 
时 间 /s 
(人 变 刚 度 控 制 器 对 关节 2 轨迹 的 跟踪 结果 
图 9.45 ( 续 ) : 
0.2 一 跟踪 误差 
| --- 跟踪 误差 幅 值 边界 
二 : 
中 
六 -0.1| 
A 
0 5 10 15 20 
时 间 /s 


(a) 带 误 差 转换 的 控制 器 对 关节 1 轨迹 的 跟踪 误差 


图 9.46 三 种 控制 器 对 两 关节 角 轨 迹 的 跟踪 误差 
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0.2 一 跟踪 误差 可 
时 0. 跟踪 误差 幅 值 边界 
洲 -0.1 
-0.2 
0 5 10 15 20 
时 间 /s 
(b) 带 误差 转换 的 控制 器 对 关节 2 轨迹 的 跟踪 误差 
0.2 跟踪 误差 og 
及 0.1 跟踪 误差 幅 值 边界 
所 
亲 
0 5 10 15 20 
时 间 /s 
(c) FAT 控 制 器 对 关节 1 轨迹 的 跟踪 误差 
0.2 -一 跟踪 误差 
引 01 跟踪 误差 幅 值 边界 
R 中 
六 -0.1 
0 
0 5 10 15 20 
时 间 /s 
(d) FAT 控 制 器 对 关节 2 轨迹 的 跟踪 误差 
一 跟踪 误差 


一 跟踪 误差 幅 值 边界 


关节 1/rad 


0 5 10 15 20 
时 间 /s 


(e) 变 刚度 控制 器 对 关节 1 轨迹 的 跟踪 误差 


0 一 跟踪 误差 
i 0 -跟踪 误差 幅 值 边界 
六 ol 
A 5 10 15 20 
时 间 /s 
(f 变 刚度 控制 器 对 关节 2 轨迹 的 跟踪 误差 
图 9.46 ( 续 ) 


2. 导 纳 控制 性 能 仿真 

该 实验 主要 是 对 导 纳 控制 性 能 进行 验证 。 实 验 中 ,外 部 力矩 施加 于 机 械 辟 6 一 16s, 导 纳 
控制 用 来 跟踪 受 外 部 力矩 影响 的 修正 参考 轨迹 ,并 由 观测 器 基于 广义 动量 法 对 其 进行 估计 。 
实验 结果 如 图 9. 49 一 图 9. 52 所 示 。 如 图 9. 49 所 示 ,关节 w0 的 期 望 轨迹 g, 将 通过 外 部 力矩 


a 


30 


20 


神经 网 络 权 值 范 数 


0 5 10 15 20 
时 间 /s 


图 9.47 两 关节 神经 网 络 权 值 范 数 的 变化 


一 关节 1 
0 
E 
el 
[~ 
二 
-30 5 10 15 20 
时 间 /s 
(a) 关节 1 的 输入 力矩 
2 
一 关节 2 


0 5 10 15 20 
时 间 /s 
(b) 关节 2 的 输入 力矩 


图 9.48 控制 器 的 输入 力矩 


进行 修正 ,使 机 器 人 具有 和 柔性 行为 。 由 于 外 力矩 作用 于 垂直 方向 , 且 关 节 el 的 运动 轨迹 为 水 
平方 向 , 故 不 会 改变 关节 el 的 期 望 运动 轨迹 。 导 纳 控 制 下 的 跟踪 性 能 如 图 9. 50 所 示 -, 导 纳 控 
制 下 的 跟踪 误差 如 图 9. 51 所 示 ,对 比 图 9. 50 和 图 9. 51 可 知 ,通过 观测 器 得 到 的 外 力 的 估计 
值 和 动量 的 估计 值 均 接近 于 它们 的 真实 值 ,说 明 观 测 器 估计 外 力 的 方法 是 有 效 的 。 外 部 力矩 
的 估计 值 如 图 9. 52 所 示 。 对 比 图 9. 44 中 操作 者 通过 Kinect 传感器 产生 的 参考 轨迹 、 图 9.51 
和 图 9. 52 的 虚线 可 知 , 当 机 械 辟 受 到 外 力 时 , 导 纳 控制 可 以 改变 机 械 臂 的 预 设 运动 轨迹 ,并 产 
生 一 条 修正 轨迹 ,以 避免 机 械 臂 与 外 界 产生 碰撞 ,从 而 达到 与 外 界 环 境 的 顺应 交互 。 


ls 


一 期 望 轨迹 
--- 修 正轨 迹 


0 5 10 15 20 
时 间 /s 
9.49 ”期 望 轨迹 与 修正 轨迹 
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一 修正 轨迹 
--- 真 实 轨迹 


0 5 10 15 20 
时 间 /s 


9.50” 导 纳 控 制 下 的 跟踪 性 能 


小 一 也 际 训 吴 差 
0.1[\ -跟踪 误差 幅 值 边界 


幅 值 /rad 


0 5 10 15 20 
时 间 /s 


图 9.51 导 纳 控制 下 的 跟踪 误差 


1 一 真实 力矩 
-估计 力矩 


外 部 力矩 幅 值 /N-m 


5 10 15 20 
时 间 /s 


图 9.52 外 部 力矩 的 估计 值 


9.5 基于 输入 饱和 的 机 械 璧 增强 导 纳 控制 


本 节 将 介绍 一 种 基于 神经 网 络 的 机 械 臂 自 适应 导 纳 控制 方法 ,解决 控制 系统 中 的 输入 饱 
和 问题 。 

近 三 十 年 ,机 器 人 广泛 应 用 于 各 种 领域 ,如 教育 .工业 和 娱乐 等 。 这 些 应 用 大 多 要 求 机 器 
人 能 够 实现 良好 的 机 器 人 -环境 交互 性 能 。 传 统 的 力 /位 置 控制 系统 不 能 满足 安全 、 快 速 、 准 确 
交互 的 要 求 。 阻 抗 控制 是 一 种 经 典 的 方法 ,该 概念 最 早 是 由 霍 根 所 出 的 , 它 涉及 力 和 末端 执行 
器 位 置 的 调节 。 男 一 种 实现 机 械 辟 柔性 行为 的 方法 是 由 马 森 提出 的 导 纳 控制 。 导 纳 控制 的 思 
想 是 通过 轨迹 自 适 应 实现 机 械 辟 的 柔性 行为 。 导 纳 控 制 的 关键 是 外 部 力矩 的 测量 。 

传统 的 获取 外 部 力矩 信息 的 方法 是 使 用 力 传感器 , 它 固定 在 机 械 臂 的 末端 执行 器 上 。 然 
而 ,这 些 方法 会 给 机 器 人 系统 带 来 很 多 不 便 。 近 年 来 ,人 们 对 无 传感器 末端 执行 器 扭矩 估计 方 
法 进行 了 大 量 的 研究 ,并 提出 了 许多 方法 。 因 此 ,这些 算 法 被 广泛 地 集成 到 涉及 物理 机 器 人 与 
环境 交互 的 控制 系统 中 。 

在 实际 应 用 中 ,硬件 上 的 物理 输入 饱和 是 许多 控制 系统 中 的 一 个 问题 。 在 许多 执行 机 构 
中 ,控制 系统 不 可 避免 地 要 受到 一 定 的 约束 。 当 存在 饱和 非 线性 情况 时 ,输入 饱和 可 能 导致 较 
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大 的 跟踪 误差 和 不 稳定 性 。 为 了 解决 这 一 问题 ,人 们 做 了 大 量 的 工作 和 研究 。 这 些 算法 性 能 
良好 ,在 许多 控制 系统 中 得 到 了 应 用 。 

通常 机 器 人 的 控制 方法 分 为 基于 模型 的 控制 和 无 模型 的 控制 。 第 一 种 方法 包括 比例 导数 
(Proportional Derivative,PD) 方 法 .比例 积分 导数 (Proportional Integral Derivative, PID) 方 
法 和 学 习 方 法 。 基于 模型 的 控制 包括 计算 力矩 控制 方法 、 自 适应 控制 方法 等 。 传统 的 基于 模 
型 的 控制 方法 具有 良好 的 跟踪 性 能 。 然 而 ,这 些 方法 在 很 大 程度 上 依赖 于 系统 模型 的 准确 性 ， 
在 实际 应 用 中 又 往往 无 法 得 到 一 个 准确 的 系统 模型 。 因 此 ,设计 一 种 能 够 补偿 输入 饱和 的 控 
制 系统 是 非常 重要 的 。 由 于 机 器 人 系统 的 机 理 复 杂 , 智 能 控制 方法 (如 基于 神经 网 络 的 控制 ) 
受到 了 广泛 的 关注 。 神 经 网 络 具 有 全 局 逼近 的 能 力 , 可 用 于 处 理 未 知 的 机 器 人 系统 动力 学 问 
题 。 通 过 在 线 学 习 确 定神 经 网 络 的 参数 。 本 节 将 介绍 一 种 机 械 臂 与 环境 交互 时 的 无 传感器 控 
制 方案 。 采 用 基于 广义 动量 的 和 干扰 观测 器 代替 力 传感器 来 估计 机 械 臂 末端 执行 器 受到 的 环境 
外 力 ,并 设计 了 一 个 用 于 轨迹 跟踪 的 自 适应 神经 控制 器 。 


9.5.1 饱和 约束 问题 描述 

考虑 系统 是 饱和 非 线 性 不 确定 系统 , 它 代表 一 类 非 线 性 系统 。 幅 值 饱 和 描述 的 是 执行 器 
的 输入 和 输出 之 间 的 幅 值 关系 。 当 输入 信号 在 某 一 区 间 的 时 候 , 输 
出 信号 可 以 与 输入 信号 保持 一 致 。 但 是 , 当 输 入 信号 超过 某 一 个 幅 
值 时 ,输出 信号 将 趋 近 于 一 个 常数 , 且 随 着 输入 信号 的 幅 值 增加 , 幅 
值 并 不 增 大 , 当 发 生 此 类 现象 的 时 候 , 称 执行 器 输入 饱和 。 饱 和 度 
是 一 个 静态 非 线性 函数 ,用 来 表示 信号 的 超过 上 限 的 不 敏感 现象 ， 
如 图 9. 53 所 示 , 输 入 饱和 区 域 为 1 二 ts,t 二 ti ,增加 输入 信号 的 幅 
值 ,输出 信和 号 趋 于 常数 。 饱 和 现象 的 数学 描述 可 以 表示 为 : 


图 9.53 输入 饱和 约束 


be u 之 T max 
SY g(t) 2 


T u < Tmin 
其 中 ,wu 是 输入 信号 ,g (1) 是 光滑 的 函数 ,S,, (7) 表 示 输 出 非 线 性 关系 。z,, 表示 饱和 非 线 性 
的 最 大 值 ,r,s 表示 饱和 非 线 性 的 最 小 值 。 

9.5.2 观测 器 设计 

n 连 杆 的 机 器 人 的 动力 学 公式 为 : 


M(g)gi+C(g,9)9+G(g)=r ron | (9. 128) 


min 


由 式 (9. 87) 可 知 
p=M(g)9 +M(g)g 
将 式 (9. 87) 代 入 式 (9. 128) 中 可 得 : 


b=sC'(g,9) +rt—G(g) tr (9. 129) 

首先 ,定义 观测 器 的 误差 。=p 一 ,观测 器 的 方程 为 : 
B=C'™(g,9) +r—G(gq)+K,e (9. 130) 

其 中 ,K， 是 一 个 对 角 和 矩阵 。 比 较 式 (9. 129) 与 式 (9. 130), 并 定义 r= 二 Ke ,可 得 : 
i 一天，( 方 —$P)=K, (ro —r7) (9. 131) 


将 式 (9.131) 变 换 到 拉 普 拉 斯 域 . 


R(s) Pn 7,) 
下 


pi 


第 9 章 ”其 他 控制 方法 | 及 207 


由 上 式 可 知 ,将 外 力矩 信号 通过 一 个 低 阶 滤波 器 可 以 得 到 : 
r=K,(p—$)=K,(p 一 | cr +r—G(g) +r))d 
其 中 ,K， 为 增益 矩阵 。 
9.5.3 控制 器 设计 


本 节 将 介绍 自 适 应 神经 网 络 控制 器 的 设计 以 及 其 稳定 性 分 析 。 
首先 ,定义 误差 跟踪 变量 ; 
jE 三 8 二 人 
a 


, 05 = Ke 
其 中 ,K 是 增益 矩阵 。 考虑 外 部 扰动 ， 设计 控制 力矩 为 ， 
Te 十 G 十 Ma 十 Ca —t ,+ K, (e, +é)+K, sgn(e,) 

其 中 ,K， 和 K, 是 控制 增益 和 矩阵 ,YC(g)、C(g,9) 和 G’(g) 是 RBF 神经 网 络 的 估计 项 ,& 为 状 
态 变 量 。 

下 面 ,考虑 带 输 入 饱和 的 n 连 杆 的 机 器 人 动力 学 公式 : 

M(g)qg+C(g,90)9 +G(g)=S (rT) ro (9, 132) 

式 中 ,S,, (rt) 如 前 述 已 经 定义 ,此 处 不 再 闭 述 

将 控制 力矩 代入 式 (9. 132) 中 可 得 : 

—M(g)é,=—(M(g) —M(g))a — (Cl(g,9) —C(g,d))a — (G(g) —G(g))— 


eu tC 0 .Art ri 直 (eKyvsgn(ed) (9. 133) 
其 中 : 
(一 也 
Ar, 一 Zext = 
| el Ar | 十 0.5ArIAr 
> 2 v 
| K: el 十 Arz 当中 6l 兰 六 时 (9 134) 
当 el <w 时 
式 中 ,K. 表示 增益 矩阵 ,w 为 很 小 的 正 实数 。 
用 RBFNN 通 近 未 知 项 : 
M(g) =WHhSm (gq) 
Clg,9) =WLSc(g,d) (9. 135) 
GC(gq) =WiSo (gq) 
设计 RBFNN 的 更 新 律 : 
jn =Ou (Sumie, 二 oem Wu) 
Wc =@c (Seae, +ocWe) (9. 136) 


凤 。 =O@6c (Soe, +ocWoe) 
其 中 ,Ov \@- 和 @- 均 为 正 实数 矩阵 ,c ( 全 ) 是 很 小 的 正 实数 矩阵。 
将 式 (9. 135) 和 式 (9. 136) 代 入 式 (9. 133) 得 : 
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-Mtoe == Ce EA Nd TE 


K,sgn(e,) WuUSmé —WLSca —WESG (9. 137) 
其 中 ,Wa 二 全 —Wy ,We =Wce —We ,We =We —W, o 
设计 李 雅 普 诺 夫 函 数 如 下 : 
Te 
V = ee 十 了 evMe。 党 和 十 了 trCWYwBw Wi) 十 
Ftr WiOc We) 十 Ftr WIG Wo) (9. 138) 


对 式 (9. 138) 求 导数 . 
V=esé, +esMi, 十 Fe TMe, +éTé+tr(WIOmWWy) 十 
tr(WI@c Wc) + tr(WIOa We) (9. 139) 
将 式 (9. 137) 代 和 人 上 式 可 得 : 


和 T。 


多 一 ev6y 一 ev (一 cy 十 Ce +K,(e,t+é)+K;sgn(e,) t+Ar++Ar, +é s+ 


os < ew a 
WhiSméi +WiSca t+WLSG 十 二 eIMe + tr (Whie, —omWhWau)+ 


2 
tr(Wuiae, —ocWEWe) 十 tr(Wee, —ocWELWe) (9. 140) 
将 RBFNN 更 新 律 代 人 上 式 得 : 
一 eye 一 el (一 evy Ce Me,(e,é) 二 +K,sgn(e,) 十 Ar 十 Are) 十 6 十 
了 ceTWre， —tr(ov WhiWu) —tr(locWEWe) — tr(ooWEW,) (9. 141) 


利用 Young 不 等 式 , 并 结合 式 (9. 136) 得 : 


一 | eAr | 一 了 ArTAr 十 6 Ar 


1 
5 
基于 Young 不 等 式 , 可 将 式 (9. 141) 化 简 为 : 
V 委 eye 一 e: (一 ev 十 Ce 十 开 。(ev 十 6) 十 开 ,sgn(e) 十 Ar 十 Ar。) 一 
ET'K.é—| elAr + TE — tr(ov Wh Wu) We 
tr(oeWEWe) — tr(ocWEW,) (9. 142) 
由 式 (9. 130) 可 得 : 
V 委 eel —el(—e, +K,(e,té))—esAr, —é Keé—|erAr I+ 也 一 
tr(ov WuWau) — tr(ccWEWe) — tr(ccWEW,) (9. 143) 
化 简 式 (9. 143) 可 得 : 


, 1 1 
VR eré, eoKe, 十 eves — elK,e,e!K,é+ es 十 FAre Ar。 > EK.é 区 
1 a i sy 

FP —tr(ov WiuWu)—tr(ccWEeWe) — tr(ccWeW,) (9. 144) 


化 简 式 (9. 144) 得 : 
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VelKe, 一 F642Ks —I—KIK,)é—e!l(K,—I)e,+C ” (9. 145) 
式 中 : 
C=—tr(ov WuWu) — tr(ocWEWe) — tr(occWEW,) 十 FArIAr, (9. 146) 
要 想 稳 定 系统 ,还 需要 满足 : 
2K.—I—KiK, 宇 0 


(9. 147) 
K, 一 TI 三 0 


根据 UUB 稳定 分 析 , 当 系统 满足 如 下 不 等 式 : 
也 6T(2K —I—KIiK,)é 
一 ev 天 ev 十 ev (K,— 1)e; >C 
V<0 时 ,系统 稳定 ,系统 中 的 误差 项 趋 于 一 个 不 变 集合 。 
9.5.4 仿真 验证 
本 节 基 于 两 连 杆 的 机 械 臂 对 上 述 控制 器 仿真 验证 。 机 械 臂 的 动力 学 模型 如 式 (9. 85) 所 
示 , 机 械 辟 参数 为 ; mj =2kg, ms 一 2kg,7l 一 0. 2m1y 一 0 2m; Tj =0. 027kgm’, I, = 
0.027kgm” ,g 二 9.81m/s 。 设 仿真 时 间 ty 二 10s。 
1. 力矩 观测 器 仿真 
在 机 械 臂 末端 执行 器 上 施加 外 力矩 共 2 一 10s。 动 量 p 和 外 力矩 r.。 的 实际 曲线 和 期 望 


曲线 的 结果 分 别 如 图 9. 54 和 图 9. 55 所 示 。 可 以 看 出 ,由 观测 器 估计 的 力矩 比较 接近 实际 力 
矩 , 各 关节 的 广义 动量 误差 可 趋 于 零 ,说 明 设 计 的 观测 器 的 估计 性 能 良好 。 


5 | ER 实际 力矩 
一 实际 动量 - - 估计 力矩 
三 -一 估计 动量 
至 0 
ES 
0 2 4 6 8 10 0 2 4 6 10 
时 间 /s 时 间 /s 
一 实际 动量 0.5 ~- 估计 力矩 
三 -一 估计 动量 三 
兢 0 0 
-0.5 
0 2 4 6 8 10 0 2 6 10 
时 间 /s 时 间 /s 
图 9.54 动量 p 的 实际 曲线 和 期 望 曲 线 图 9.55 外 力矩 rss 的 实际 曲线 和 期 望 曲 线 
2. 导 纳 控制 器 仿真 


导 纳 控制 器 仿真 参数 设置 如 下 : 输入 参考 轨迹 qv 二 [cos(1) ,sin(1)]' ,机 械 辟 关节 角 的 
初始 值 g 二 [1.0,0.0]' ,关节 角速度 的 初始 值 一 [0,0]”。 选 取 式 (9. 104) 所 示 的 导 纳 模型 ， 
参数 选取 My 二 diag(1.0)、Cy 二 diag(1.0)、Gy 二 diag(10.0), 仿 真 结果 如 图 9. 56 所 示 。 由 该 
图 可 知 , 当 外 力矩 作用 于 机 器 人 机 械 臂 2 一 10s 时 ,修改 所 需要 的 轨迹 g, 来 调整 外 力矩 ,然后 
生成 虚拟 目标 轨迹 g, 并 跟踪 。 期 望 轨迹 的 跟踪 结果 如 图 9. 57 所 示 ,由 跟踪 结果 可 以 看 出 ,9 
和 qs 能够 有 效 地 跟踪 虚拟 期 望 轨迹 。 控 制 输出 力矩 如 图 9. 58 所 示 , 各 节点 神经 网 络 权 值 列 
向 量 的 范 数 如 图 9. 59 所 示 。 从 仿真 结果 可 以 看 出 ,该 控制 方法 具有 良好 的 控制 性 能 。 
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d2/rad 


一 修正 参考 轨迹 4 一 实际 轨迹 9， 
=“ 参考 轨迹 gw 1 ~ - 修正 参考 轨迹 gq, 
1 一 ~ 人、 号 
0 0 
| | 
0 2 4 6 10 0 和 4 6 8 10 
时 间 /s 时 间 /s 
2 一 修正 参考 轨迹 49， 一 实际 轨迹 gq， 
, ~ - 参考 轨迹 gm 1 ~ -修正 参考 轨迹 q,， 
Wy 
0 了 县 0 
™ 
| | 
0 4 6 10 0 4 6 10 
时 间 /s 时 间 /s 
图 9.56 参考 轨迹 gq, 和 修正 参考 轨迹 g， 图 9.57 机 械 臂 的 实际 轨迹 g 和 修正 参考 轨迹 g， 
E 40 
忆 
最 30 
下 20 
至 
未 10 
六 0 
Tin le a a i, 
时 间 /s 
E 15 
尼 
和 10 
3 
至 
中 0 
米 -5 
下 


I 


3. 输入 饱和 的 仿真 
这 里 定义 两 个 关节 的 输入 饱和 上 下 界 分 别 为 [一 36N，m,36N。mj,[ 一 30N，。，m,30N.， mj]。 


时 间 /s 
图 9.58 控制 输出 力矩 


— WW 


— WW, 


0 1 2 3 4 5 6 ‘ 8 9 
时 间 /s 


图 9.59 各 节点 神经 网 络 权 值 列 向 量 的 范 数 


关节 1 的 控制 输出 力矩 如 图 9. 58 所 示 ,关节 1 的 控制 力矩 输出 大 于 36N。m。 图 9. 59 为 神 
经 网 络 权 值 列 向 量 的 范 数 。 输 入 饱和 补偿 器 的 主要 作用 是 使 控制 力矩 值 为 30 一 36N * m。 
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图 9. 60 为 带 输 入 饱和 补偿 器 的 控制 力矩 ,由 图 可 知 ,使 用 输入 饱和 补偿 器 之 后 ,控制 力矩 的 幅 
值 小 于 36N.m。 跟 踪 性 能 如 图 9. 61 所 示 ,跟踪 误差 如 图 9. 62 所 示 。 上 述 仿真 结果 验证 了 
该 神经 网 络 控制 器 的 有 效 性 。 


一 如 


~-- 饱 和 边界 


0 1 2 3 4 


i i 
时 间 /s 


图 9.60 带 输入 饱和 补偿 器 的 控制 力矩 


md 
一 “dil 


时 间 /s 
图 9.61 带 输 入 饱和 的 神经 网 络 控制 器 的 跟踪 性 能 


0.04 t 
一 跟踪 误差 z11 
号 
三 
总 
Eee 2 4 6 8 10 
时 间 /s 
一 跟踪 误差 z1> 
号 
至 
总 


时 间 /s 
图 9.62 带 输 入 饱和 的 神经 网 络 控制 器 的 跟踪 误差 


一 一 一 
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9.6 基于 人 工 势 场 法 的 全 向 移动 机 器 人 路 径 规划 


本 节 将 介绍 一 种 基于 人 工 势 场 法 对 全 向 移动 机 器 人 (OMR) 进 行 运动 轨迹 规划 的 算法 。 

近 几 十 年 ,全 向 移动 机 器 人 受到 越 来 越 多 的 关注 。OMR 采用 全 向 轮 的 优点 之 一 是 它 没 
有 差分 驱动 移动 机 器 人 中 存在 的 非 完 整 约束 问题 。 通 过 输入 各 个 全 向 轮 的 转速 ,移动 机 右 人 
可 以 轻松 地 移动 到 用 户 想 要 的 任何 位 置 。 这 简化 了 控制 律 ,易于 实现 。 全 向 轮 由 轮 和 滚 子 组 
成 , 即 整个 全 向 轮 的 速度 是 轮 速 和 滚 子 速度 的 组 合 。 为 使 得 移动 机 器 人 获得 全 向 移动 性 能 ,至 
少 需要 3 个 全 向 轮 ,而 常见 的 全 向 移动 机 器 人 结构 有 三 轮 结构 和 四 轮 结构 ,如 图 9. 63 所 示 。 


全 向 轮 


N 


2 


三 轮 结构 


多 
全 向 移动 机 器 人 四 轮 结构 和 


全 向 移动 机 器 人 | 


图 9.63 三 轮 和 四 轮 OMR 结构 图 


机 器 人 的 控制 非常 复杂 ,有 时 需要 考虑 机 器 人 的 状态 约束 来 完成 控制 设计 。 本 节 中 全 向 
移动 机 器 人 结构 为 使 用 3 个 Garbowiecki 轮 搭建 移动 机 器 人 ,后 述 所 建立 的 动力 学 .运动 学 模 
型 以 及 实验 平台 , 均 是 基于 如 图 9. 64 所 示 的 OMR。 如 图 9. 65 所 示 为 基于 四 个 麦克 纳 姆 轮 拱 
建 的 全 向 移动 机 器 人 。 


全 向 轮 


图 9.64 三 轮 OMR 外 观 图 图 9.65 四 轮 OMR 外 观 图 


9.6.1 全 向 轮 运动 学 和 动力 学 建 模 

1. 全 向 轮 坐 标 系 

全 向 轮 由 轮 圾 和 从 动 轮 组 成 ,从 动 轮 和 轮 载 的 运动 方向 夹 角 为 we, 二 90",*r, 为 轮 载 半径 
(i 二 1,2,3)。 本 节 使 用 的 全 向 移动 机 器 人 如 图 9. 66 所 示 。 由 图 可 知 ,三 个 轮 为 中 心 对 称 分 
布 ,其 中 第 i 个 全 向 轮 坐 标 系 与 机 器 人 坐标 系 的 夹 角 为 "8; (i 二 1,2,3) ,第 i 个 全 向 轮 质心 到 
机 器 人 质心 的 长 度 用 7, 表示 。 
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图 9.66 OMD 轮子 分 布 及 坐标 系 


2. 摄像 头 坐标 系 

摄像 头 按 功 能 可 分 为 RGB 摄像 头 ( 一 般 用 来 采集 图 像 ) 和 深度 摄像 头 ( 可 采集 到 物体 至 摄 
像 头 之 间 的 距离 ) 。 摄 像 头 采集 环境 信息 范围 如 图 9. 67 所 示 。 如 华硕 XTION2 深度 摄像 头 
具体 参数 如 表 9. 1 所 示 。 由 表 9. 1 可 知 ,该 深度 摄像 头 的 传感器 不 仅 包含 深度 摄像 头 , 还 包含 
RGB 摄像 头 。 


图 9. 67 摄像 机 采集 环境 信息 范围 


表 9.1 华硕 XTION2 深度 摄像 头 具体 参数 


采集 的 有 效 距离 0. 8 一 4m 

采集 角度 (74°,52" ,90°> 

传感器 深度 传感器 和 RGB 传感器 
深度 传感器 640X 480@30FPS 

RGB 传感器 分 辩 率 1280X720@P60 

使 用 环境 室内 


3. 全 向 轮 运动 学 建 模 
由 图 9. 66 中 的 全 向 轮 坐 标 系 可 得 式 (9. 148)。 


i 0 sinkasy "w, 
wy > | "vi 
其 中 ,v;, .v;, 和 w; 是 笛 卡 儿 坐 标 系 中 点 ”O 的 广义 速度 "vi 、 "vi 和 "ui 是 点 ”O 在 坐标 


系 "X"O"Y 中 的 广义 速度 。"v; 是 第 i 个 全 向 轮 的 速度 分 量 ,a; 为 全 向 轮 和 其 从 动 轮转 轴 之 
间 的 夹 角 。 


"w; 
| (9. 148) 


r 


vs 
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图 9. 68 是 全 向 移动 机 器 人 的 坐标 系 图 ,由 图 9. 68 可 
得 "vw, 和 ”"w, 在 OMR 坐标 系 中 的 关系 ,如 下 。 


Ts Cos(sBy) =”sink Ba Uys 
一 95 1497 
"Viy smn( By ‘COS BY Viy 


由 式 (9. 148) 和 式 (9. 149) 可 得 


加 区 — stn( hp | 网 RE 四 
= 有 Ri 一 有 ?Ri 
sin(2B;) cos(7B) Ty "Vi 
《9. 150) 
其 中 ,vie 和 "uiy 分 别 是 "vi; 在 "X 轴 和 "Y 轴 方向 的 速度 分 量 ,"B; 为 全 向 轮 坐 标 系 与 机 器 人 


坐标 系 的 夹 角 ,RzR1 是 全 向 轮 坐 标 系 到 机 器 人 坐标 系 的 转换 矩阵 。 
不 考虑 机 器 人 在 垂直 方向 上 的 运动 , 即 机 器 人 在 全 局 坐标 系 中 进行 平面 运动 ,因此 有 


图 9.68 OMR 坐标 系 


的 二 区 .大 
“ha LO lh, , 
= 项 一 及 3 2 Co L511Y 
| 
"yy 区 
其 中 
人 eos pL 
一 (C9; 152) 
hs sin( Bd: 


“xz 和 "vw, 分 别 是 机 器 人 在 OMR 坐标 系 xz 轴 和 y 轴 的 速度 ,"w 为 OMR 的 角速度 , "1; 
和 "1;, 分 别 是 "1; 在 OMR 坐标 系 中 xz 轴 和 y 轴 的 方向 分 量 。 
由 于 式 (9. 150) 和 式 (9. 151) 左 侧 相同 ,可 得 


be "w; y 5 
| -a ry (9.153) 


全 向 轮 运动 学 模型 可 以 通过 将 式 (9. 153) 左 右 两 边 同 时 左 乘 [LR?R1] “得 到 。 


7 


vi 


"wi pF kL. 
| [mr 人 (9. 154) 
vi 


令 


R* = [Ri “ERR: 


Ee casa —Sin(as)| | os Be) sint BY| AL = 
“FSC | ~ 0 Sn By ‘CGCB 和 NAO A 


cos("B; —a;) sin("B, — a;) "Li sin(TB; — ai) —"li,ccos("B; —a;) | 


1 


SC “rsin(&,) “r,sin(a;) 
cos("B;) sin(®%B; ) — C0 Bt Lh Sin Bi) 
sin(a;) sin(a;) sin(a; ) 


(9. 155) 
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考虑 全 向 轮 角 速度 "w; ,只 取 R; 的 第 一 行 , 即 可 得 OMR 运动 学 模型 ,由 式 (9..152) 可 得 


i v, 
:| (9. 156) 
3 rw 
其 中 ,4， 为 
| ecos(Cp8 一 ai) sin(2p 一 ai) “Lisin(al) 十 "liccos(al) 
“r,sin(ai) “r,sin(a1) “risin(al) 
cost Bi — ay) sin( BB —asy "lsin(as)t Lceosta;) 
二 rt i -| .1D 
cos(T Bs 一 xs) Sint BB — ds) "Lsintas) tt Lsceos(ns) 
| rssin(as) “rssin(as) “rin(as) 
根据 角速度 和 速度 的 转换 公式 v 一 rm ,可 以 将 式 (9. 156) 写 成 如 下 形式 。 
"vi 外 Fr 
"oz| 一 “7r4Ai | 一 "74 |’y (9. 158) 
Ls ‘w . 
区 cos("0) 一 sin(70) 0| |'z TI, 
y|=|sin(0) cos(’0) 0| yl+ |y, (9. 159) 
0, 0 0 国史 0 
对 式 (9. 159) 求 导 得 
| 本 is 
y|=|sin("0) cos(’0) 0 'y = 和 > “>y (9. 160) 
0, 0 0 Uo 0 
其 中 
cos("0) 一 sin(" 0) 0 
A2= |sin("0) cos(’0) 0 (9. 161) 
0 0 1 
将 式 (9. 160) 代 入 式 (9.161) ,可 得 速度 的 逆向 运动 学 模型 
a , 
| 站 2 坊 (9. 162) 
oa 六 
同时 ,还 可 得 到 角速度 的 逆向 运动 学 模型 如 下 。 
Fey a 
a (9. 163) 
"ws 0 


实际 搭建 的 OMR 运动 学 模型 参数 如 表 9. 2 所 示 。 
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表 9.2 OMR et 


将 表 9. 2 中 数据 代入 式 (9.157) 和 式 (9. 161) 中 ,可 得 
"wi ee 0. 00987 区 


0 "00197 这 ,3823| 中 这 (9. 164) 


0.0170 0.00987 2.3323 


则 雅 可 比 矩 阵 4; 为 
一 0.0170 0.00987 2.3323 一 0.0170 0.00987 2.3323 
A -| 0 一 0.0197 ”名 多 交 |=| 0 —0.0197 2.3323 
0.0170 0.00987 2.3323 0 0 6. 997 
根据 式 (9. 165) ,可 得 rnt A 由 雅 可 比 矩 阵 满 秩 可 知 , 该 OMR 可 以 实现 全 方位 的 
运动 。 
4. OMR 动力 学 建 模 
OMR 的 中 心 点 'O 在 坐标 系 XOY 中 的 点 坐标 为 (z,,y,)。 由 牛顿 第 二 定律 下 一 zza， 


可 得 : 


(9. 165) 


(9. 166) 


本 =m, "a, =m,y, 
式 中 ,“ 下 ,和 下, 为 力 ? 下 分 别 在 X 和 立方 向 上 的 分 量 ( 其 中 ,“F 为 OMR 中 心 点 *O 受到 的 
力 ); “a, 和 "a, 是 ca(OMR 加 速度 ) 分 别 在 X 和 YY 方向 上 的 分 量 。m, 是 移动 机 器 人 的 
质量 。 

定义 矩阵 M==diag(m,m), 则 式 (9. 166) 与 式 (9. 168) 等 价 。 


6F =MS (9. 167) 


Ea 0 
一 ， F= 避 (9.168) 
yr RS ‘ A 


由 XOY 坐标 系 和 ”XX "OY 坐标 系 的 转换 公式 ,可 得 


其 中 


$=T’$, $=T'§+Tr$, SF=T’'F (9. 169) 

其 中 ,"S 和 "F 是 $ 和 SF 在 OMR 坐标 系 "X7"O7"Y 下 的 映射 。 将 式 (9. 169) 代 入 式 (9. 167) ， 
可 得 : 

"F=M(CS+T T'S (9. 170) 


考虑 角 加 速度 和 角速度 对 OMR 的 作用 ,并 结合 式 (9. 170) ,可 得 OMR 的 运动 学 模型 如 下 : 
m, (2, —y,0,)="F, 


my, — 0,) ="F, C9 71 
.0 =M; 
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其 中 ,TI 是 OMR 重心 绕 中 心 轴 的 转动 惯量 ,Mj 是 作用 在 OMR 上 关于 其 重心 轴 的 力矩 。 由 
式 (9.172) 可 得 "下 ,和 ”下 , 分 别 为 : 
"F,=Nssin(x—,B,) + Nssin( Bs CO— 7) 
(9. 172) 
“pF ENTE Necos(n Be) Nscos(s Bs — A) 
OMR 第 i 个 全 向 轮 的 驱动 系统 的 动力 学 模型 可 以 用 式 (9.173) 表 示 
T.6 十 好, 一) (9. 173) 
其 中 ,NY (i 二 1,2,3) 是 第 i 个 全 向 轮 的 驱动 力 ,& 是 地 面 与 全 向 轮 之 间 的 摩擦 系数 ,IT,, 是 全 
向 轮 绕 其 中 心 轴 的 转动 惯量 ,h 是 一 个 驱动 系数 。 
全 向 轮 的 速度 "vw; 可 由 “r"w; 计算 。 全 向 移动 机 器 人 的 动力 学 模型 可 以 写成 如 下 形式 : 


六 =A(X)X+B(X)u (9. 174) 
其 中 
下 ee 
(9; 175) 
区 =[r rt rs] 
| 0 . | 
31, 十 2 or” 31, 2m,°r’ 
PR i 6 
31, 十 2 or” 31,2m,°r’ 
0 和 
Bh Nd 
< (9. 176) 
— he hrx] 2h"“rcos(0,) 
3T,+2m,r: 3T. 十 2 or 31,++2m,”r? 
hrk, hrks 2h"rsin(0,) 
DM Se a 
1 -2 3 mF 831. 下 27 
Wa hy a 


3T 7 有 十 Ter 人 和 3T 二 Tr 31 +I, er’ 

并 令 ki = V3 sin(0.) 一 cos(0.),x? 一 一 V3sin(0,)—sin(0,),r,= V3cos(0,)—sin(0,)。 

9.6.2 ”人工 势 场 法 

人 工 势 场 法 路 径 规 划 是 由 Khatib 提出 的 一 种 虚拟 力 法 。 它 的 基本 思想 是 将 机 器 人 在 周 
围 环境 中 的 运动 设计 成 一 种 抽象 的 人 造 引力 场 中 的 运动 ,目标 点 对 移动 机 器 人 产生 “引力 ”, 障 
碍 物 对 移动 机 器 人 产生 “ 斥 力 ”( 如 图 9. 69 所 示 ), 最 后 通过 求 合力 来 控制 移动 机 器 人 的 运动 
(如 图 9. 70 所 示 )。 应 用 势 场 法 规划 出 来 的 路 径 一 般 比 较 平滑 并 且 安 全 ,但 是 这 种 方法 存在 局 
部 最 优点 问题 ; 且 人 工 势 场 法 无 法 将 机 器 人 自身 的 一 些 约束 考虑 进 路 径 规 划 上 ,如 机 器 人 的 
最 大 运动 速度 等 。 为 了 解决 这 个 问题 ,可 以 将 模型 预测 控制 器 用 于 全 向 移动 机 器 人 的 运动 控 
制 , 以 补偿 人 工 势 场 法 规划 路 径 的 不 足 。 

目标 点 对 移动 机 器 人 产生 的 “引力 ”会 引导 移动 机 器 人 运动 ,而 障碍 物 对 移动 机 器 人 产生 
的 “ 斥 力 ? 则 会 驱使 移动 机 器 人 远离 障碍 物 ,最 终 移动 机 器 人 在 斥 力 与 引力 的 共同 作用 下 移动 ， 
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如 图 9. 70 所 示 。 


~ 
、 
AN 


~ 


a 
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" 厅 J 
、 
\ 各 部 并 


图 9.69 虚拟 势 场 图 图 9.70 人 工 势 场 法 合力 图 


利用 人 工 势 场 法 对 OMR 进行 路 径 规划 时 ,会 生成 一 个 虚拟 的 力 引导 移动 机 器 人 的 运动 ， 
同时 躲避 障碍 物 。 为 了 便于 分 析 ,将 移动 机 器 人 视 为 在 二 维 空间 中 运动 的 质点 ,其 位 置 可 表示 
为 S$ 二 [x,，,y,]"。 超 声波 传感器 可 以 检测 机 器 人 与 目标 、 机 器 人 与 障碍 物 之 间 的 距离 和 角 
度 。 由 目标 点 产生 的 引力 势 场 函 数 可 以 由 式 (9.177) 计 算 。 
Uls) 一 CattC (s,sgo0al) (C9. 217) 
其 中 ,a 是 一 个 正比 例 系 数 ,d “(s,s6on) 二 (sooa 一 s) 是 OMR 质点 和 目标 点 之 间 的 距 
离 ,c 二 1 或 2。 当 c=2 时 ,引力 为 : 


Pus2=— VU = amd (380) CQ. L783 
斥 力 势 函 数 为 : 
1 入 
ao 一 一 一 一 d"(s,s,0l) 当 d(s;s,o) 三 do 时 
| Prep. Z| a 2 (9.179) 


0 要 志学 时 
其 中 ,ae 是 一 个 正比 例 系 数 ,ds,sobs) 是 OMR 质点 与 障碍 物 之 间 的 最 短 距 离 ,n 是 一 个 正 
系数 。d。 表示 障碍 物 对 移动 机 器 人 的 影响 程度 , 它 是 一 个 正 的 常数 。 
斥 力 为 : 


Fpnor 二 FnrG 当 d(s,s 之 do 时 


goal ) 


Fs(s) 一 一 YU (Cs) = 


(9. 180) 
0 当 dts,s a) > do: 时 
其 中 ,nog= 二 Vd(s sss NnRG 二 Vd (s,sgoa) 分 别 是 从 障碍 物 点 指向 OMR 以 及 从 OMR 指向 
目标 点 的 单位 向 量 。 结 合式 (9. 178) 和 式 (9. 180) ,可 得 合力 下 ow 的 计算 公式 如 下 : 
Bils) Pt) FE, Cs) 
=P (tne Fg ls) no th ees Re (9. 181) 
根据 式 (9. 181) 可 以 计算 出 合力 角 gou。 假 设 OMR 的 位 姿 9, 已 知 , 则 由 式 (9. 182) 可 得 
0, 和 oa 的 差 值 0, o 
0. 一 0 — Orotal (9. 182) 


9.6.3 全 向 移动 机 器 人 的 模型 预测 控制 器 设计 
1. 模型 预测 控制 
近年 来 ,模型 预测 控制 (Model Predictive Control, MPC) 在 运动 控制 、 物 联网 中 得 到 了 广 
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a 


泛 的 应 用 。MPC 对 模型 的 精度 要 求 较 低 ,适用 于 阶 跃 响应 模型 .线性 和 非 线 性 模型 。.MPC 的 主 

要 思想 是 将 控制 问题 描述 为 一 个 代价 函数 的 优化 问题 ,在 特定 的 约束 条 件 下 ,使 代价 函数 最 小 化 

的 输入 就 是 最 优 输 入 。MPC 的 思想 与 具体 的 模型 无 关 , 但 是 实现 则 与 模型 有 关 。 一 般 情况 下 ， 

MPC 是 由 模型 预测 .滚动 优化 和 反馈 校正 3 个 模块 组 成 ,如 图 9.71 所 示 为 MPC 的 基本 原理 。 

在 & 时 刻 对 被 控 对 象 系统 的 输出 进行 测量 , 则 可 得 测量 值 y(k), 以 下 面 的 状态 空间 模型 为 例 。 
zk 二 1)=f(r(k) ,uCR)) 


(9. 183) 
y(k)=g(zr(k) ,ulk)) 


已 知 系统 的 初始 状态 x (0)= 二 x ,其 中 ,x (k)ER” wu(k)ER”? 和 y(k)ERR"” 分 别 表示 系 
统 & 时 刻 的 状态 、 输 入 和 输出 。 定 义 i 时 刻 的 输出 为 y, (十 ilk), 则 未 来 p 时 刻 内 所 有 的 输 
-出 为 yp Ck 十 1|k) ,yp Ck 十 21&),… ,yp《k 十 pk), 对 应 的 输入 为 二 [Lu Ck1kR),uCk 十 1|k),…， 
u(k 十 bp 一 11k)]。 需 要 注意 的 是 ,这 里 的 & 时 刻 输入 与 十 1 时 刻 的 输出 相对 应 。 

对 于 MPC, 系 统 的 输入 x 、 输 出 y 以 及 4d 都 要 满足 一 定 的 约束 条 件 , 即 : 

Wo < UE Wax 
yn EYE SS ym 
dunin dk 十 i) dar 

最 终 最 优 控制 输入 的 问题 就 转化 为 输出 与 参考 值 差 值 最 小 的 问题 , 即 图 9.71 中 阴影 面积 
最 小 。 

k++p 
J (y RUCR)) = >) C00) —y, Ci |k)’) (9. 184) 
i 一 A 十 1 

1) 模型 预测 

模型 预测 控制 应 具有 预测 功能 , 即 能 够 根据 系统 当前 时 刻 的 控制 输入 以 及 过 程 的 历史 信 
息 ,预测 过 程 输出 的 未 来 值 ,因此 ,需要 一 个 描述 系统 动态 行为 的 模型 作为 预测 模型 。 预 测 模 
型 具有 展示 过 程 未 来 动态 行为 的 功能 ,这 样 就 可 像 在 系统 仿真 时 那样 ,任意 地 给 出 未 来 控制 策 
略 ,观察 过 程 不 同 控制 策略 下 的 输出 变化 ,从 而 为 比较 这 些 控制 策略 的 优 劣 提供 基础 。 

2) 滚动 优化 

模型 预测 控制 是 一 种 优化 控制 算法 ,需要 通过 某 一 性 能 指标 的 最 优化 来 确定 未 来 的 控制 
作用 。 与 通常 的 离散 最 优 控制 算法 不 同 , 它 不 是 采用 一 个 不 变 的 全 局 最 优 目标 ,而 是 采用 滚动 
式 的 有 限时 域 优化 策略 ,如 图 9.72 所 示 。 滚 动 优 化 的 优化 过 程 不 是 一 次 离线 完成 的 ,而 是 反 
复 在 线 进行 的 , 即 在 每 一 采样 时 刻 , 优 化 性 能 指标 只 涉及 从 该 时 刻 起 到 未 来 有 限 的 时 间 , 而 到 

一 个 采样 时 刻 ,这 一 优化 时 段 会 同时 向 前 。 这 种 优化 算法 在 每 一 个 时 刻 有 一 个 相对 于 该 时 
刻 的 局 部 优化 性 能 指标 。 


Hl Kt2 ktm Hp 
图 9.71 模型 预测 控制 的 基本 原理 图 9.72 滚动 优化 原理 图 
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3) 反馈 校正 


在 模型 预测 控制 中 ,通过 输出 的 测量 值 y, 与 模型 的 预 估 值 
y 进行 比较 ,得 出 模型 的 预测 误差 ,再 利用 模型 预测 误差 来 对 模 
型 的 预测 值 进行 修正 ,如 图 9.73 所 示 。 

2. 基于 运动 学 模型 的 模型 预测 控制 器 

结合 式 (9. 162) .v= 二 rw .OMR 在 机 器 人 坐标 系 "X7O7Y 的 位 姿 信息 "X=[ "zx,,"y,,”0,]' 
以 及 三 个 全 向 轮 速度 "v 一 ["vi， usrvs] ,可 得 : 


图 9.73 反馈 校正 原理 图 


ro 一 及 17 放 (9. 185) 
将 式 (9. 185) 左 右 两 边 同 时 左 乘 *ri :41 1 ,可 得 
"A (9. 186) 
通过 零 阶 保持 器 ,可 将 连续 时 间 系 统 描 述 为 如 下 离散 时 间 形 式 : 
"XCG 二 1)="'X() 二 +'X()T (9. 187) 


其 中 ,T 为 采样 周期 。 结 合式 (9. 164) 和 式 (9. 186) , 式 (9. 187) 可 写成 下 述 形式 ， 
rz (1 十 1) rz (ji) 一 1.1547Trui — 0.05785T"w, 十 0.5777Trus 


"y,(j 十 1)|= +y C1002Tw; O38TTv; (9. 188) 
人 "0,(j) 十 0.0028Trvs 
定义 MPC 的 代价 函数 为 : 
j 十 N 一 1 
JI(CX.ro) 一 >， LELK(CXG)roGED) 十 FGXCG 十 N))) (9. 189) 
k=j 
式 中 ,Lk 为 
N 
I DE eo es 
k=1 
N,-—1 
DPD) AroT(R 十 JRKkAro( 十 7 17) (9. 190) 
k=0 


其 中 : NN 达 1 为 预测 边界 ,1 二 NN, 三 N 是 控制 边界 ,Qk 和 Rk 是 权重 矩阵 ，X (CR 十 7 17 ) 为 

OMR 的 预测 位 姿 ,A"v(k 十 j 17) 是 控制 器 的 输入 增 量 ,Lk (7X， ,从 丰 归 党 用 的 标准 二 次 型 形 

式 。 在 线 求解 有 限 边 界 最 优 控制 问题 如 下 : , | 
"wy" =argminJ (Xsv) (9. 191) 


可 得 7 时 刻 的 输入 "7 ) ,由 于 电机 产生 的 力矩 受 电机 性 能 的 限制 ,zz ” 有 上 限 和 下 限 , 且 "v” 
的 变化 也 会 受到 约束 。 则 
人 人 


IA Van SA (9. 192) 


eK 鸭 全 < 
正 向 运动 学 方程 (9. 188) 可 以 用 如 下 形式 描述 : 
"X(T1)=g1CXO)) gC XG) vw) (9. 193) 


其 中 ,gi 和 g， 是 连续 的 非 线性 函数 。 g1(0)=0 ,9 一 [7 Tr.， "es eo 是 状态 向 量 , 可 得 


max 
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r 


2 一 1.1547 一 0.5785 0.57777 
(a ys 坊 20 二 下 | 而 —1.002 0.334 (9. 194) 
rg 0 0 0. 0028 
定义 如 下 向 量 : 
"及 二 人 区 ND] ER 
ra i | (9. 195) 
A 
预测 输出 为 : 
"X 一 GA7TG ) 十 g1 ge (9. 196) 
其 中 
sal RN 0 人 0 
ee EC XN wal RCN) vs 0 
gC RENARD " Bat NNNOY oa Bat XCNN YY 
a a (9. 197) 
A Gd, gs X(N1))'v(i—1) 
We CERN, ,= Wo Se c RaN 
pC RN gol X(NN))'v(i CO— 1) 


式 中 ,入 ;一 7 一 1 十 117 一 1。 
式 (9.189) 的 最 优化 问题 可 以 转换 成 如 下 形式 ， 
minC| AXOG) NG 十 1AzG)1R =min (GA'TG) tg1 tg2 | + DATG) NR ) 


(9. 198) 
式 (9. 198) 约 束 如 下 
A vi SS MV) EO 
的 vl) = Sn 
(9. 199) 
ka vi 一 1) 十 TAroG) 委 Ar 
i 过 GD) 二 Tg1 +g, ES 
式 (9. 198) 的 优化 问题 也 可 以 转换 成 一 个 二 次 规划 问题 ， 
(FA'T' WA + HTA'D) (9. 200) 


其 中 
W=2(G'QkG+Rk) 
H=—2G'Qxk (gi +g2) (9. 201) 
E=[—I I —G G] 
3. 基于 动力 学 模型 的 模型 预测 控制 器 
由 OMR 的 动力 学 模型 可 得 式 (9. 202) 如 下 : 
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一 3T.7Y70 一 36 之 — hreiri + hreir, | 
3 
2M°r? + 31, 
ry a = +h reisrts + 2h°rrs A 
2M°*r? + 31, 
Ta 一 36710270 二 hl*r(ri 十 ts 十 Tt3) 
| (I, 31,)°r? | 
将 式 (9.188) 代 入 式 (9. 202) 中 , 则 OMR 的 离散 动力 学 可 写成 如 下 形式 : 
Ls 二 人 =) 二 全.T 
ry 十 1) ="3,0)+"y,T (9. 203) 


"Bt 在 


tt 了 es 
根据 力矩 r 公式 + 二 一 一 个 一 和 转速 公式 vw 二 一 ,可 推导 出 : 功率 PP 确定 的 情况 下 ,r 与 
v 满足 下 面 的 关系 

-~ 
19100r XP Xx_k a By 

60v U 

将 式 (9.174) 式 (9.186) 、 式 (9. 203) 和 式 (9. 204) 结 合 ,可 得 

"XG ="XOG)FACK)r AT — TBCXIIT (9. 205) 


ulj) 
约束 如 下 : 
| < V1) SE Vmax 

SR (9. 206) 

ie A ) EA 
其 中 ,min 和 max 分 别 表示 对 应 参数 的 下 限 和 上 限 ,v 为 速度 ,u 为 输入 力矩 ,Au 表示 是 力 拢 
的 变化 率 。 
式 (9.205) 的 预测 模型 可 以 写成 : 


U min 


A RI HAF RE 
uj +k—1|; 


人 十 四 [7 这 平王 7 十 且 CXOzG +h—1| DT 


(9. 207) 
其 中 ,EL1,Ni]。 


结合 上 述 分 析 ,根据 预测 边界 N 和 控制 边界 N， ,可 得 OMR 状态 与 输入 力矩 x 之 间 的 二 
人 


入 


N。 
5 | GDXG HE Du TG tI RouG + —1|7)) 
k=1 


(9. 208) 


其 中 ,Qjp 和 Rb 分 别 是 对 应 的 权重 矩阵 。 
因此 ， Re 
w =argmnin, Baty (9.209) 
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9.6.4 仿真 与 结果 

这 里 设计 的 仿真 环境 是 一 个 大 小 为 100mX100m 的 二 维 空间 ,其 中 包含 6 个 障碍 物 O; (i 二 
1,2,3,4,5,6), 如 图 9.74 所 示 。 设 OMR 的 仿真 起 点 和 目标 点 分 别 为 (5,90) 和 (70,30) ,起 始 
速度 为 m/s。 定 义 预测 边界 为 N= 二 5, 控 制 边界 为 N, = 二 3。 由 于 运动 学 控制 器 和 动力 学 控制 
器 的 输入 不 同 , 所 以 约束 条 件 也 不 同 。OMR 与 障碍 物 之 间 的 距离 是 可 调 的 ,日 OMR 模型 的 
参数 值 是 根据 实际 搭建 的 OMR 数据 确定 。 本 节 基 于 MATLAB 平台 进行 仿真 。 


长 度 /m 
图 9.74 仿真 环境 


1. 运动 学 控制 器 仿真 

首先 基于 OMR 的 运动 学 模型 进行 仿真 ,结合 模型 预测 控制 和 人 工 势 场 法 对 OMR 进行 
导航 。 在 此 仿真 中 ,假设 目标 位 置 已 知 ,并 且 提 前 设 定 初始 位 置 和 初始 速度 。 仿 真 过 程 如 
图 9.75 所 示 。 首 先 , 确 定 了 移动 机 器 人 的 起 始 位 置 和 起 始 速度 。 然 后 ,将 目标 位 置 发 送 给 移 
-pt 基于 人 工 势 场 法 ,移动 机 器 人 可 以 绘制 出 下 一 个 状态 ,作为 MPC 的 参考 。 在 MPC 

程 结束 后 ,将 预测 状态 发 送 给 移动 机 器 人 进行 导航 ,并 将 预测 状态 发 送 给 上 一 个 过 程 为 下 一 
py 基于 MPC 和 人 工 势 场 法 ,OMR 可 以 调整 速度 ,最 终 到 达 图 9.76 和 图 9.77 中 
的 目标 点 (0,; 表示 第 i 个 障碍 物 )。 从 图 9.76 和 图 9.77 可 以 看 出 ,OMR 成 功 地 到 达 目 标点 ， 
并 且 能 够 顺利 地 躲避 障碍 物 。OMR 的 速度 如 图 9.78 一 图 9. 80 所 示 。 


参考 控制 


o 人 工 ”| 拟 迹 点 | 模型 预测 | 输入 ,| ”移动 
控制 器 机 器 人 
: 机 器 人 
位 置信 息 长 度 /m 


图 9.75 仿真 流程 图 图 9.76 仿真 环境 下 运动 学 模型 控制 器 仿真 效果 
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90 
80 1.6 
70 
日 和 1.4 
和 
于 2 
40 
300 20 40 60 80 0 20 40 60 80 
X/m X/m 
图 9.77 全 向 移动 机 器 人 运动 轨迹 图 9.78 全 向 移动 机 器 人 的 速度 
1.0 
1.6 0.5 
全 
'w 1.4 网 
和 室 -05 
这 
12 过 =10 
时 = 
0 20 40 60 80 的 20 40 60 80 
时 间 /s 时 间 /s 
图 9.79 全 向 移动 机 器 人 的 速度 变化 图 9.80 移动 机 器 人 三 个 全 向 轮 的 速度 


在 仿真 开始 时 ,将 机 器 人 的 起 始 速度 设 为 1。 当 移 动机 器 人 开始 运动 时 ,MPC 将 速度 修 
改 为 w 。 从 图 9.76、 图 9.78、 图 9.79 可 以 看 出 ,v 变化 较 快 的 时 刻 是 移动 机 器 人 遇 到 障碍 物 
需要 改变 方向 的 时 刻 。 图 9. 80 为 三 个 全 向 车 轮 的 速度 。 根 据 人 工 势 场 法 ,可 以 得 到 总 的 力矩 
角 ,并 确定 7 的 值 ,7 的 变化 如 图 9. 81 所 示 。 

基于 人 工 势 场 法 ,OMR 可 以 顺利 地 避 开 障碍 物 ,并 成 功 到 达 目 标点 。 基 于 模型 预测 控制 
器 ,可 以 约束 移动 机 器 人 的 运动 速度 ,同时 也 增强 了 系统 的 鲁 棒 性 。 

2. 动力 学 控制 器 仿真 

动力 学 模型 控制 器 仿真 ,由 人 工 势 场 法 和 模型 预测 控制 器 导航 与 运动 学 相同 的 过 程 ,仿真 
结果 如 图 9. 82 一 图 9. 86 所 示 。 从 图 9. 82 和 图 9. 83 可 以 看 出 ,基于 OMR 动力 学 控制 器 和 人 
工 势 场 法 的 模型 预测 控制 器 , 当 OMR 接近 障碍 物 时 ,人 工 势 场 法 会 产生 一 个 斥 力 ,使 OMR 
改变 方向 ,成 功 躲 避 障 碍 物 并 到 达 目 标点 。 将 图 9. 84、 图 9. 85、 图 9. 82 进行 比较 ,不 难看 出 ， 
当 速 度 急剧 变化 时 ,OMR 过 于 接近 障碍 物 ,需要 减速 以 避免 可 能 发 生 的 碰撞 。 从 图 9. 86 可 
以 看 出 , 当 OMR 需要 改变 方向 时 , 它 会 分 别 改变 3 个 力矩 ,从 而 产生 一 个 合力 来 引导 它 的 
运动 。 

由 上 述 仿 真实 验 结果 可 知 ,基于 三 轮 OMR 的 运动 学 模型 和 动力 学 模型 搭建 的 模型 预测 
控制 器 能 有 效 地 跟踪 人 工 势 场 法 产生 的 轨迹 。 
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0.5 
0 
咏 
8 -0.5 
二 
-1.0 
ng 20 40 60 80 
X/m X/m 
图 9.81 由 人 工 势 场 法 确定 的 7 图 9. 82 动力 学 模型 全 向 轮 在 工作 空间 的 运动 仿真 
90 1.15 
80 1.10 
70 = 
E60 二 1.00 
50 0.95 
40 0.90 
20 20 40 60 80 0850 20 40 60 80 
X/m X/m 
图 9. 83 动力 学 模型 全 向 轮 的 运动 轨迹 图 9.84 动力 学 模型 全 向 轮 的 运动 速度 
1.15 
1.10 E 
a 
1.05 下 
六 100 站 
仿 
0.95 二 
le 
0.90 
0.830 10 20 30 40 50 60 0 TE 
时 间 /s 时 间 /s 
图 9.85 ”动力 学 模型 全 向 轮 的 速度 变化 图 9. 86 动力 学 模型 全 向 轮 的 力矩 变化 


9.7 基于 牛顿 - 欧 拉 模型 的 自 适应 迭代 学 习 控 制 


随 着 机 器 人 的 发 展 ,机 械 制造 、 娱 乐 、 医 疗 保健 、 人 机 协作 等 诸多 社会 应 用 得 到 了 极 大 的 改 
善 。 机 器 臂 主要 应 用 于 工业 领域 ,其 工作 环境 对 人 身 安全 是 有 威胁 的 ,机 器 人 和 人 应 该 分 别处 
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于 不 同 的 区 域 ,以 确保 人 身 安全 不 会 受到 机 器 人 高 刚度 的 威胁 。 此 外 ,在 某 些 情况 下 ,机 器 人 
只 能 通过 改变 自身 的 刚度 来 实现 所 需 的 动力 学 。 

目前 ,在 一 些 实际 应 用 中 ,传统 的 基于 位 置 控制 的 机 器 人 在 安全 方面 面临 很 多 问题 。 一 方 
面 ,机 械 臂 的 交互 环境 是 不 确定 的 .复杂 的 ,导致 环境 模型 不 完善 ,甚至 对 实际 环境 一 无 所 知 。 
另 一 方面 ,现在 大 多 数 机 器 人 都 是 为 特定 接触 点 的 固定 环境 设计 的 。 当 机 械 臂 采用 这 种 策略 
工作 时 ,在 柔顺 控制 任务 中 ,已 经 设置 好 的 位 置 控制 器 会 不 断 地 拒绝 由 交互 作用 产生 的 力 ,最 
终 导 致 更 大 的 交互 作用 力 。 在 某 些 实际 应 用 中 ,这 种 特性 可 能 会 对 人 类 协作 者 造成 伤害 。 为 
了 解决 这 个 问题 ,相互 作用 力 必须 限定 在 一 个 合理 的 范围 内 。 通 常情 况 下 ,可 以 通过 采用 非 冲 
突 的 方式 来 控制 力 和 位 置 ,或 者 通过 阻抗 控制 方法 建立 交互 力 和 机 械 臂 位 置 之 间 的 关系 ,来 保 
证 机 器 人 的 柔顺 运动 。 与 其 他 控制 方法 不 同 ,阻抗 控制 更 倾向 于 利用 目标 阻抗 模型 来 调节 机 
械 阻抗 。 

随 着 现实 世界 的 不 确定 性 、 复 杂 性 的 不 断 增加 和 越 来 越 多 的 复 用 , 越 来 越 需要 一 种 安全 、 
柔顺 的 学 习 控 制 方法 。 

9.7.1 问题 描述 

本 节 主 要 介绍 迭代 阻抗 模型 ,以 获得 对 未 知 环境 的 最 优 交互 性 能 。 在 接 下 来 的 讨论 中 ,分 
别 考虑 两 种 不 同类 型 的 环境 。 

1. 动力 学 建 模 与 识别 

1) 动力 学 建 模 

本 节 将 使 用 牛顿 - 欧 拉 (NE) 公 式 对 机 器 人 进行 动力 学 建 模 。 传 统 拉 格 朗 日 方法 通过 显 式 
分 析 机 器 人 系统 的 力 . 运 动 和 动能 ,得 到 回归 矩阵 S(q,9,9,,4,)。 然 而 , 随 着 机 器 人 自由 度 
的 增加 , 拉 格 朗 日 方法 的 计算 成 本 显著 增加 ,因此 拉 格 朗 日 方法 无 法 对 一 个 自由 度 大 于 4 的 机 
器 人 机 械 臂 进行 建 模 (例如 ,KUKAiiwa7R800 机 器 人 有 7 个 自由 度 )。 使 用 NE 公式 可 以 解 
决 这 一 问题 , 且 利 用 NE 公式 可 以 提高 控制 器 的 计算 效率 。 

利用 NE 公式 ,可 以 将 机 器 人 动力 学 计算 分 为 两 步 : 正 向 递归 和 反 向 递归 。 正 向 递归 可 


以 计算 如 下 : 
lg FT ;Rp 十 ziGi 
pi = Ro i Ro, 1 X20: vd; (9. 210) 
De “RL J Pp a alway Re PY 


其 中 ,; “PER” 是 变换 向 量 ,p;ER* 是 角速度 ,%; ER” 是 角 加 速度 ,而 ;ER 是 连 杆 ;在 . 
坐标 平面 内 的 线 加 速度 ,z; 是 关节 旋转 轴线 的 向 量 。 
然后 ,由 连 杆 i 的 运动 引起 的 在 关节 i 处 的 力 和 由 连 杆 i 一 1 的 运动 引起 的 在 关节 i 处 的 
力矩 可 以 由 NE 反 向 递归 公式 计算 如 下 : 
fa mp tp Xm tp (pi Xm 
(C9211 


na =T9: T+ pi XTigi) — Ei Xm 
其 中 ,&; 二 0; 十 pg Xv;。 乘 积 符号 L(。) 和 辅助 矩阵 玉 (。) 分 别 为 : 
Se i a 
ppXh=L(g)h= | 9 0 0 Ws lls 9 2 
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We 

i 

"I Py eR 0 g 
Ig=E(pR(DD=|0 9p: 0 9，9。 0 Se (9. 213) 

VD pe OV dll 

: 

er 


其 中 ,hER” 是 一 个 常数 向 量 ,同时 TIER” 是 一 个 惯性 矩阵 。 然 后 将 式 (9. 212) 和 式 (9. 213) 代 
入 ,可 将 式 (9. 211) 改 写 为 ， 


(9. 214) 
miéi + Lp Om tL oI)L pm 


然后 , 式 (9. 214) 可 以 通过 对 m; my; 和 证 的 线性 参数 化 改写 得 到 以 下 形式 : 


EY a 
| en t | 


mi; 
: me Tn 


ma | = C9. 215) 
é; 工 (Pi) 十 过 (Pi) 瑟 (Pi) 0 RI,) 


其 中 ,A; 是 一 个 6X10 矩阵,@; 为 10 个 未 知 惯性 参数 的 矢量 ,&; 是 关节 i 处 完全 取决 于 连 杆 
i 的 运动 的 力 /力矩 ,CT,)= 王 [LTz ,Ty ,Toz,Ty， Tvz， Tez] 是 惯性 参数 的 矢量 。 其 中 6; 可 
以 由 以 下 方程 获得 : 


N 
§; = 2 (9. 216) 
j=i 
5 二 了 工序 (9.217) 
其 中 : 
Re Reeth 
Te 条 ES (9. 218) 
HR 0 


9 es 
由 式 (9. 216) 和 式 (9. 218) 可 知 ,上 述 机 器 人 的 串联 运动 链 可 表示 为 ， 
6 Qi Ql … Qu 12， 


也 = S Qa . Qs (9. 219) 
EN 0 0 四 央 了， 
然后 将 力矩 / 力 矢 量 映射 到 关节 旋转 轴 上 ,得 到 施加 在 每 个 关节 轴 上 的 力矩 ; 
el 
FE] 
0 
CQ 220) 


s ' 四 © 


其 中 ,zx, 一 [0,0,1]" 是 第 i 个 关节 旋转 轴 的 向 量 ,r, 是 关节 转 矩 ,Sy ER'”' 是 矩阵 Qi 对 应 
的 行 。 
然后 ,可 以 得 到 机 器 人 的 线性 参数 内 动力 学 如 下 : 
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Si St ™ Si | 
r 一 SG 一 Be ee Sm 机 (9.221) 
0 0 Sn 0, 
其 中 ,rz 一 [rry，…r,] 是 机 器 人 关节 力 抢 的 矢量 = 
2) 机 械 臂 模型 
机 器 人 运动 学 可 由 下 式 描 述 
X(t)=$(g) (9..222) 
根据 上 述 NE 模型 ,一 个 n 连 杆 机 器 人 系统 的 动态 可 以 描述 如 下 : 
M(g)g+C(g,9)9 =S$(g.9,9)B +G(g)=r (9, 2232 


其 中 ,x (i) 表示 在 笛 卡 儿 空 间 中 的 位 置 ,g ER" ,n 代表 机 械 辟 的 自由 度 (DOF)。 
然后 对 式 (9. 222) 求 导 , 可 得 式 (9. 224) : 
i(t)=J(g)g (9. 224) 
其 中 ,J (一共 ER"” 为 包含 机 器 人 直线 和 角速度 的 雅 可 比 矩阵 。 
将 式 (9. 224) 对 时 间 求 导 , 得 到 : 
i(t) = (go + (gq)g (9. 225) 
其 中 ,f(z:)ER"*" 表示 机 器 人 机 械 臂 与 环境 之 间 存 在 的 相互 作用 力 矢量 ,在 没有 机 器 人 和 环 
境 间 相互 作用 时 ,这 是 一 个 零 矢 量 。 
此 外 ,参考 式 (9. 221) . 式 (9. 223) 和 式 (9. 224) ,可 以 将 上 述 机 器 人 动力 学 转化 为 适应 操作 
空间 的 形式 ， 
M- (gC (qs te (0) = f(t (9. 226) 
其 中 
M,(g)=J ' (gM(gq)J Cd) 
C, (gqg,0)=J "(gq)(C(g,9) — M(g)J (gqg))J (gq) 
G,(q)=J (gq)G(g) 
re i Oe 
同时 ,M,, (gq) 区 、C,, (gq,9) 和 G, (9g) 满足 : 
M, (q)¥+C(go)i+G, (gq) =Y( t,t,T)0 (9, S28 = 
其 中 ,9ER” 和 ng 分 别 表示 机 器 人 机 械 辟 的 物理 参数 向 量 和 这 些 参 数 的 个 数 ; Y(%, ,二 , , 均 ， 
xz)ER”” 是 回归 和 矩阵; 区 , 和 牵 , 将 由 式 (9. 240) 定 义 。 
3) 阻抗 模型 | 
为 了 使 下 面 的 分 析 更 加 方便 ,除非 另 有 说 明 ,否则 系统 参数 和 信和 号 都 依赖 于 时 间 。 为 了 应 
用 柔性 控制 ,将 阻抗 模型 表示 如 下 : 
My(o)z 十 Cdo,o)z 十 Gv(Cdo)e 一 让 (9. 229) 
其 中 ,zy 是 在 笛 卡 儿 空间 中 的 期 望 轨迹 ,而 e=zv 一 z 是 跟踪 误差 。Mu .Cys 和 Gu。 分 别 表示 
期 望 的 惯性 .阻尼 和 刚度 和 矩阵。 需要 特别 指出 的 是 ,在 这 个 阻抗 模型 中 ,可 以 根据 不 同 的 应 用 
需求 来 选择 M, .Cy 和 Gy 的 最 优 值 。 这 就 意味 着 ,在 某 些 交互 应 用 中 ,需要 修改 这 些 参数 ,以 
便 在 交互 控制 任务 期 间 获 得 最 佳 兼 容 性 能 。 参 考 模 型 即 式 (9. 226) 建 立 了 位 置 误差 与 交互 力 
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三 之 间 的 期 望 动态 关系 。 当 机 器 人 不 与 环境 接触 时 ,交互 力 f 为 零 向 量 。 在 这 种 情况 下 ,只 
要 式 (9.226) 稳 定 ,机 械 臂 的 实际 轨迹 就 会 收敛 到 参考 轨迹 。 由 于 这 一 特性 ,本 节 所 介绍 的 模 
型 结合 了 两 种 任务 情况 (交互 力 f 是 否 为 零 向 量 ) ,以 避免 两 种 不 同 模式 之 间 的 赢 振 。 

4) 控制 目标 

本 节 的 控制 目标 是 控制 力矩 ,以 获得 式 (9. 226) 的 最 优 柔 度 性 能 。 

为 了 更 清晰 地 定义 上 述 控制 目标 ,将 阻抗 误差 定义 为 : 


0 (9. 230) 
通过 在 每 次 迭代 时 对 所 有 +E€[L0,tyj] 重 复 这 些 动 作 , 可 以 得 到 如 下 迭代 学 习 控制 律 : 
imow (2) >0 Vit€ LO (9. 231) 


_ 其 中 ,ktj 分 别 表 示 和 迭代 次 数 和 和 迭代 周期 。 由 于 环境 的 复杂 性 ,缺乏 足够 的 环境 信息 ,导致 系 
统 模型 的 不 完善 。 针对 这 一 问题 ,可 以 采用 一 种 迭代 学 习 控制 方法 来 解决 。 通 过 使 用 一 组 具 
有 指定 操作 条 件 的 迭代 操作 ,搜索 所 需 的 控制 输入 。 

9.7.2 学 习 控制 器 设计 

1， 控制 器 设计 

增 广 阻抗 误差 定义 为 : 

=Rw = RK —K,e titRKr (9. 232) 

其 中 ,Ks Mj!Cy ,K, 一 Mi!Gi ,K; 一 Mz1。 

假设 两 个 正定 矩阵 A 和 卫 满足 以 下 方程 : 


人 十 卫 一 玉 ， 
(9. 233) 
TA = 
式 (9.232) 可 改写 如 下 : 
二 一 此 一 (A 十 D2 一 TU 十 六 十 TF (9. 234) 
f4 满足 : 
fit+rft=Kyf* . (9. 235) 
定义 : 
zx 一 一 给 一 人 ee 十 大 (9. 236) 
式 (9.234) 可 变换 为 以 下 紧凑 形式 : 
夯 《 一 上 + Tz* (9. 237) 
需要 注意 的 是 ,六 一 0 且 zx* 二 0, 会 导致 a* = 二 0。 
系统 控制 输入 如 下 : 
环 二 70 十 区 十 入 十 产 (9. 238) 


其 中 ,cr 和 分 别 表示 计算 转 矩 矢量 反馈 转 矩 矢量 和 补偿 转 矩 矢量 。 产 表示 /* 的 测量 值 。 
由 于 在 实际 应 用 中 力 的 精确 测量 是 很 难保 证 的 , 力 测量 噪声 闫 = 天 一 产 关 0 通常 存在 
于 许多 应 用 中 。 假设 力 测 量 有 一 一 个 已 知 的 有 和 界 约 束 。, 如 | fF* 过 e, 可 以 由 以 下 方程 获得 计 
算 转 矩 r4 : 
rt =MExt CLO =Y(2t, it ,t,x 《9. 239) 
其 中 ,6 是 9 的 估计 值 ,同时 和 ”满足 : 
i 一 zy The —f 
(9. 240) 
2 = 十 人 64 一 大 
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且 廊 满足: 
Fr +rf = (9. 241) 
反馈 力矩 矢量 可 由 下 式 求 得 : 
z 名 二 一 天 过 (9. 242) 
其 中 ,K 为 对 称 正定 和 矩阵。z” 满足 : 
Zz* = 一 et 一 Aet 二 ft =z* tf (9. 243) 
i 
补偿 力矩 由 下 式 给 出 : 
rt =—K .sgn(z*) (9. 244) 


其 中 ,K ,之 &, 它 将 在 接 下 来 的 部 分 分 析 ,rt 用 来 表示 补偿 力 测量 的 误差 。 
将 系统 控制 输入 (9.238) 代 入 式 (9. 226) ,闭环 动力 学 可 表示 为 : 
M,(q*)z* +C, (gd )z* 十 Kz =Y(2t, it, i rt) — (Ksgn(z)—f*) 


(9. 245) 
其 中 ,0* =0*—0* ,并 假定 : 
ed a (9. 246) 
同时 ,和 矩阵 S 是 对 称 正定 的 。 
2. 控制 性 能 分 析 
分 别 把 式 (9. 238) 和 式 (9. 246) 描 述 的 系统 控制 输入 和 和 迭代 学 习 规 律 应 用 ,机 械 系 统 具 有 
如 下 性 质 : 


(1) 对 于 所 有 1EL0,ty], limw' (1) 是 由 M$ 界定 并 且 通 过 力 的 高 精度 测量 ,$= 二 0 意味 
着 limw (1) 一 0。 

(2) 对 于 所 有 的 上 之 0, 财 环 中 的 所 有 信号 都 是 有 界 的 。 

证 明 过 程 如 下 。 

对 于 所 有 的 1€E[0,t yj 和 ,定义 一 个 性 能 指标 函数 如 下 : 


kk tgT 
”> -|'? (rz)dr (9 247) 
通过 定义 5 不 一 多” 一 外 , 式 (9. 243) 可 以 写成 如 下 形式 : 
a C9. 248)-: 


考虑 闭环 动力 学 (9. 245) 和 式 (9. 248) ,可 得 如 下 表达 式 : 
AU 一 7 = 


=| 9° Cr)sot (rydr — | 0 (7)SO* Cr) de 
A 5k AkT 广 开 
=| 8 (rz)S554(r) 十 255 (7r)SO*Cr)) dr 
RE 广大 
-| ce TS “YY B22 YH)de 
(g*)z*t 十 C (gq* ,9*)z* 十 


m 


-| Cn Be 


Kz* (Ksgn(z*)—f*)))dr 
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a. 


=—2| i oe -| (2K — YS 1Y* )ztdr— 
0 


“二 大 k xk 
| (Ksgnls") —f -dr (9.249) 
考虑 z* (0) 二 0, 利 用 M, (gq*) 一 2C, (gq* ,9*) 的 斜 对 称 特性 ,可 以 得 到 : 


AU* =—z* RM (q*)z —| Mo )dr -| (Kisgn(z*) —f*)dr <O 


m 


(9. 250) 
其 中 ,Z* (Ksgn(z*) 一 A) 之 0。 
由 0 和 人 有 界 ,可 得 : 
Ey =| 8" (ryS9 Cr)dr oy YY € Tosi,] (9. 251) 
根据 式 (9. 250) 和 式 (9. 251) ,可 以 得 到 : 
lim AU* -> 0 (9. 252) 
有 一 cc 
和 
im 元 =0 (9. 253) 
k—o0 
由 式 (9. 248) ,可 得 : 
limz* =—lim f* (9. 254) 
k— k—oo 
通过 以 上 分 析 , 可 得 如 下 方程 : 
limw* (1) =—limMyf*(t), Vt €[0,ts] 09. 255) 
k—o0 k—oo 
9.7.3 仿真 


本 节 基 于 一 个 七 自由 度 的 KUKA iiwa 7R800 机 械 辟 ,使 用 MATLAB 2016b 基于 牛顿 - 
欧 拉 模型 对 KUKA iiwa 7R800 进行 建 模 , 如 图 9. 87 所 示 。 
为 了 使 仿真 更 加 直观 和 简单 ,将 机 器 人 的 5 个 关节 固 
定 , 变 成 一 个 2 自由 度 的 机 器 人 (本 文 的 仿真 基于 机 器 人 - 
工具 箱 ,版 本 为 Cork 2005)。 
仿真 中 ,机 器 人 的 参数 如 下 所 示 : mi 二 14. 473 ,my 一 2 
1 60389kesl = = 0 = T= 0 0g wm = 
1 二 0. 5m, 其 中 的 m, i, ,Lr 分别 表示 连 杆 x 的 质量 、 
长 度 .2Z 轴 转 动 惯量 以 及 从 前 一 个 关节 到 质心 的 距离 。 
需要 指出 的 是 ,上 述 参数 仅 用 于 以 下 仿真 ,而 不 用 于 图 87 MATLAB 中 的 KUKA 
实际 的 控制 设计 和 应 用 。 人 
定义 如 下 缩写 : 
. ==sin(ql 十 qs)， cizs 三 cos(qi 十 qs)， cl 一 cos(q1) 


(9; 2567 
5S1 =sin(g1), s»=sintgs), cs, =cos(g,) 
运动 学 约束 如 下 : 
(isit lss) 一 12312 
J(g)= (9. 257) 
lici tt lsc Ll2c12 


初始 位 置 为 : x* (0)=1.0m,y*(0)=0.0m。 
在 箔 卡 儿 空间 中 的 理想 轨迹 为 : 
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war Or — 1 LO), Ya) =0, t EL thls 


(9. 258) 
假设 弹簧 的 静止 位 置 为 二 =1.2m, 则 相互 作用 力 为 ; 
让 二 
3 (9. 259) 
500(z 一 1.2), zx* 宇 1.2 
此 外 ,测量 噪声 的 交互 力 f* 设置 为 一 个 随机 数 。 
为 了 应 用 柔 度 控 制 ,仿真 时 将 阻抗 控制 模型 的 参数 设置 如 下 : 
Mj;=%X(2), Cy=10X(, G3 = (9. 260) 
其 中 ,XX (2) 表示 2X2 单位 矩阵 。 此 外 ,由 于 90"==0, 有 : 
K=1000X(2), Kr=2X(2),, S=2X(2) (9. 261) 
仿真 中 ,k 分 别 为 5、10、20 时 的 阻抗 误差 .相互 作用 力 和 跟踪 误差 分 别 如 图 9. 88 一 图 9. 90 
所 示 。 
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图 9.88 在 k=5 时 的 结果 
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图 9.90 在 k==20 时 的 结果 


从 仿真 结果 中 可 以 发 现 , 随 着 和 迭代 次 数 的 增加 ,阻抗 误差 减 小 。 这 意味 着 经 过 几 次 和 迭代 
后 ,机 器 人 的 行为 将 由 期 望 的 动力 学 控制 。 观 察 仿 真 结果 可 以 看 到 ,在 非 相 互 作用 的 情况 下 ， 
工 方向 的 跟踪 误差 收敛 到 零 。 


9.8 本 章 小 结 


本 章 先 对 神经 网 络 控制 和 自 适 应 控制 之 外 的 其 他 控制 方法 如 滑 模 控制 .模糊 控制 ,学习 控 
制 以 及 常 与 这 些 控制 方法 结合 使 用 的 径 向 基 函 数 神经 网 络 进行 简单 阐述 。 然 后 结合 当前 一 些 
比较 先进 的 研究 课题 ,针对 前 述 的 控制 方法 详细 地 阐述 了 不 同 应 用 场景 下 每 个 研究 课题 的 控 
制 器 的 设计 过 程 ,并 给 出 仿真 或 者 实验 结果 。 感 兴趣 的 读者 可 以 结合 自己 的 研究 方向 进行 深 
入 研究。 
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“< 库 用 篇 : 机 器 人 梭 制 靶 术 
在 人 机 交互 中 的 应 用 


机 器 人 控制 技术 (Robot Control Technology) 是 机 器 人 为 完成 各 种 任务 和 动作 所 执行 的 
各 种 控制 策略 。 作 为 机 器 人 的 “大 脑 ”, 机 器 人 控制 技术 的 重要 性 不 言 而 喻 。 它 涉及 的 范围 十 
分 广泛 ,从 机 器 人 智能 ,任务 描述 到 运动 控制 和 伺服 控制 等 技术 , 既 包 括 实现 控制 所 需 的 各 种 
硬件 系统 ,也 包括 各 种 软件 系统 。 

机 器 人 控制 系统 的 功能 是 根据 接收 传感器 的 检测 信号 和 操作 任务 的 要 求 , 驱 动机 械 臂 中 
的 各 个 电机 运动 ,达到 任务 所 需 的 控制 性 能 。 控 制 过 程 就 像 我 们 人 的 活动 需要 依赖 自身 的 感 
官 一 样 , 机 器 人 的 运动 控制 也 离 不 开 传 感 器 。 机 器 人 需要 用 传感器 来 检测 各 种 状态 。 机 器 人 
的 内 部 传感器 信号 用 来 反映 机 械 辟 关 节 的 实际 运动 状态 信息 ,机 器 人 的 外 部 传感器 信号 用 来 
检测 工作 环境 的 变化 信息 。 

机 器 人 控制 系统 有 以 下 基本 功能 : 

(1) 控制 机 械 尽 末端 执行 器 的 运动 位 置 ( 即 控制 末端 执行 器 经 过 的 点 和 移动 路 径 ) 

(2) 控制 机 械 辟 的 运动 姿态 ( 即 控制 相 邻 两 个 活动 构件 的 相对 位 置 ); 

(3) 控制 运动 速度 ( 即 控制 末端 执行 器 运动 位 置 随时 间 变 化 的 规律 ); 

(4) 控制 运动 加 速度 ( 即 控制 末端 执行 器 在 运动 过 程 中 的 速度 变化 ); 

(5) 控制 机 械 臂 中 各 动力 关节 的 输出 转 矩 ( 即 控制 对 操作 对 象 施 加 的 作用 力 ); 

(6) 具备 操作 方便 的 人 机 交互 功能 ,机 器 人 通过 记忆 和 再 现 来 完成 规定 的 任务 ; 

(7) 使 机 器 人 对 外 部 环境 有 检测 和 感觉 功能 。 例 如 ,工业 机 器 人 配备 视觉 \ 力 觉 、 触 觉 等 
传感器 进行 测量 、 识 别 , 判 断 作 业 条 件 的 变化 

随 着 信息 技术 和 控制 技术 的 发 展 , 机 器 人 应 用 范围 逐渐 扩大 ,机 器 人 控制 技术 正 朝 着 智能 
化 的 方向 发 展 ,出 现 了 离线 编程 .任务 级 语言 .多 传感器 信息 融合 、 智 能 行为 控制 等 新 技术 。 多 
种 技术 的 发 展 将 促进 机 器 人 智能 控制 系统 的 发 展 ,推进 了 机 器 人 智能 化 。 


机 器 人 行业 的 荐 勃发 展 , 离 不 开 先 进 的 科研 进步 和 技术 支撑 。 下 面 是 某 网 站 盘点 的 2018 
年 机 器 人 领域 最 前 沿 的 十 项 技术 。 

(1) 软体 机 器 人 一 一 柔性 机 器 人 技术 。 该 技术 是 指 采用 柔韧 性 材料 进行 机 器 人 的 研发 、 
设计 和 制造 。 

柔性 材料 具有 能 在 大 范围 内 任意 改变 自身 形状 的 特点 ,在 管道 故障 检查 、 医 疗 诊 断 、 侦 查 
探测 领域 具有 广泛 的 应 用 前 景 。 

(2) 机 器 人 可 变形 液态 金属 控制 技术 。 该 技术 指 通过 控制 电磁 场 外 部 环境 ,对 液态 
金属 材料 进行 外 观 特征 .运动 状态 准确 控制 的 一 种 技术 ,可 用 于 智能 制造 、 灾 后 救援 等 领域 。 

(3) 基于 生物 信号 的 控制 机 器 人 一 一生 肌 电 控制 技术 。 该 技术 利用 人 类 上 肢 表面 肌 电 信 
号 来 控制 机 器 臂 ,在 远程 控制 、 医 疗 康复 等 领域 有 着 较为 广阔 的 应 用 。 

(4) 机 器 人 的 皮肤 一 一 敏感 触觉 技术 。 该 技术 指 采 用 基于 电学 和 微粒 子 触觉 技术 的 新 型 
触觉 传感器 ,能 让 机 器 人 对 物体 的 外 形 、 质 地 和 硬度 更 加 敏感 ,最 终 胜任 医疗 、 勘 探 等 一 系列 复 
杂工 作 。 

(5) 机 器 人 “主动 ”说 话 一 一 会 话 式 智 能 交互 技术 。 采 用 该 技术 研制 的 机 器 人 不 仅 能 理解 
用 户 的 问题 并 给 出 精准 答案 ,还 能 在 信息 不 全 的 情况 下 主动 引导 完成 会 话 。 

(6) 机 器 人 的 心理 活动 一 一 情感 识别 技术 。 该 技术 可 实现 对 人 类 情感 甚至 是 心理 活动 的 
有 效 识别 ,使 机 器 人 获得 类 似 人 类 的 观察 理解 和 反应 能 力 ,可 应 用 于 机 器 人 辅助 医疗 康复 、 和 刑 
侦 鉴 别 等 领域 。 它 可 对 人 类 的 面部 表情 进行 识别 和 解读 ,是 和 人 脸 识 别 相 伴 相 生 的 一 种 衍生 
技术 。 

(7) 意念 操控 机 器 人 一 一 脑 机 接口 技术 。 该 技术 指 通 过 对 神经 系统 电 活动 和 特征 信号 的 
收集 、 识 别 及 转换 ,使 人 脑 发 出 的 指令 能 够 直接 传递 给 指定 的 机 器 终端 ,可 应 用 于 助残 康复 、 灾 

(8) 机 器 人 带路 一 一 自动 驾驶 技术 。 应 用 自动 驾驶 技术 可 为 人 类 提供 自动 化 .智能 化 的 
装载 和 运输 工具 ,并 延伸 到 道路 状况 测试 .国防 军事 安全 等 领域 。 

(9) 机 器 人 的 虚拟 现场 一 一 虚拟 现实 技术 。 该 技术 可 实现 操作 者 对 机 器 人 的 虚拟 膛 控 操 
作 , 在 维修 检测 、 娱 乐 体验 、 现 场 救援 .军事 侦察 等 领域 有 应 用 价值 。 

(10) 机 器 人 间 的 可 互联 一 一 机 器 人 云 服 务 技术 。 该 技术 指 机 器 人 本 身 作 为 执行 终端 , 通 
过 云端 进行 存储 与 计算 ,即时 响应 需求 和 实现 功能 ,有 效 实 现 数 据 互 通 和 知识 共享 ,为 用 户 提 
供 无 限 扩展 、 按 需 使 用 的 新 型 机 器 人 服务 方式 。 

上 述 机 器 人 控制 系统 能 够 实现 基本 的 控制 功能 ,如 控制 机 器 人 末端 执行 器 的 位 置 .速度 、 
加 速度 等 ,以 及 智能 机 器 人 控制 .机 器 人 领域 前 沿 技术 (如 柔性 机 器 人 技术 、 生 肌 电 控制 技术 、- 
脑 机 接口 技术 、 虚 拟 现实 技术 、 机 器 人 触觉 等 )。 关 于 机 器 人 控制 器 的 设计 也 有 一 些 具 体 实例 
(如 关于 柔性 机 器 人 的 控制 .基于 脑 电 信号 对 机 器 进行 控制 等 ), 而 对 于 人 机 交互 . 逐 操 作 、 智 能 
机 器 人 控制 技术 以 及 机 器 人 领域 前 沿 技术 的 具体 应 用 很 少 涉 及 。 本 篇 将 针对 机 器 人 控制 技术 
目前 比较 前 沿 的 应 用 进行 介绍 ,主要 包括 机 器 人 示 教 .、 膛 操作 和 人 机 交互 技术 。 此 外 ,本 篇 也 
对 机 器 人 控制 技术 在 其 他 方向 的 一 些 先进 应 用 进行 了 介绍 ,关于 这 些 应 用 的 具体 实例 ,虽然 比 
较 前 沿 但 是 不 全 面 , 感 兴趣 的 读者 可 以 在 本 篇 的 基础 上 ,结合 自己 的 兴趣 ,参考 其 他 相关 资料 
进行 更 全 面 的 学 习 。 | 


CHAPTER 10 


人 机 交互 或 人 机 互动 ,英文 名 称 为 Human-Computer Interaction 或 Human-Machine 
Interaction, 简称 HCI 或 者 HMI。 它 是 一 门 研究 系统 (包括 机 器 .计算 机 系统 .计算 机 软件 ) 与 
用 户 之 间 互 动 关 系 的 科学 ,包含 了 多 个 领域 的 课题 ,主要 跟 计 算 机 科学 .行为 科学 .设计 等 多 个 
研究 领域 有 关 。 人 机 交互 专注 研究 人 与 系统 (主要 为 计算 机 ) 之 间 的 接口 设计 ,让 人 与 计算 机 
之 间 有 更 加 友好 自然 的 交互 方式 。 交 互 的 内 涵 是 指 人 能 够 向 计算 机 输入 指令 ,让 计算 机 按照 
指令 做 出 相应 的 执行 动作 ,同时 把 执行 的 结果 以 一 定 的 输出 方式 让 人 感知 。 

人 机 交互 界面 一 般 是 指 用 户 可 见 的 部 分 。 在 人 机 交互 界面 上 ,用 户 可 以 与 系统 交流 ,并 对 
其 进行 操作 。 为 了 提高 系统 的 可 用 性 或 用 户 友好 性 ,在 设计 人 机 交互 界面 时 ,一 般 都 要 求 设 计 
含有 其 用 户 对 系统 的 个 性 化 理解 。 

下 面 简单 介绍 人 机 交互 的 特征 、 现 状 、 发 展 方向 以 及 与 人 机 交互 相关 的 技术 。 

1. 人 机 交互 的 特征 

人 机 交互 主要 有 以 下 三 方面 特征 。 

(1) 将 多 种 信息 整合 在 一 起 进行 传输 。 信 息 本 身 具 有 时 效 性 ,信息 传输 速度 越 快 ,效率 就 
越 高 ,因此 HCI 技术 需要 很 高 的 集成 效果 。 需 要 将 多 种 不 同 的 信息 集中 起 来 进行 统一 的 表示 
和 传输 。 

(2) 反馈 响应 具有 实时 性 。 为 了 实现 人 与 仪器 的 实时 互动 ,在 用 户 与 仪器 之 间 交 流 时 , 仪 
器 需要 时 刻 知道 用 户 在 做 什么 、 想 做 什么 ,并 快速 响应 ,为 用 户 提供 想 要 的 信息 反馈 。 

(3) 交互 过 程 具有 同步 性 。 因 为 在 人 机 交互 过 程 中 ,一 般 情况 下 ,要求 多 种 信息 数据 在 一 
定时 间 内 同时 传输 。 所 以 ,人 机 交互 设备 要 求 具有 同时 传输 多 种 信息 的 功能 ,为 了 实现 这 个 功 
能 ,需要 电子 计算 机 能 够 对 多 种 数据 进行 协调 。 

2. 人 机 交互 的 现状 

20 世纪 60 年 代 至 今 ,人 机 交互 技术 逐步 得 到 发 展 ,并 已 应 用 到 一 些 产 品 中 ,如 智能 手机 中 装 
有 GPS 定位 语音、 智能 选择 运作 模式 等 功能 。 动 作 识别 技术 在 高 仿真 游戏 ,便携式 计 算 机 、 隐 
形 技术 等 方面 广泛 应 用 ,例如 Leap Motion 和 Xbox One Kinect。 触 觉 交 互 技术 在 普通 机 器 人 、 医 
疗 机 器 人 、 仿 真 现实 环境 技术 .远程 遥控 设备 等 方面 得 到 应 用 。 语 音 识 别 技术 在 视频 音频 聊天 设 
备 .语音 拨号 .语音 控制 机 器 移动 .智能 家 居 等 场合 得 到 应 用 ,如 微 信 和 iPhone 的 Siri 等 。 眼 动 跟 
踪 技术 主要 为 患 有 语言 障碍 而 无 法 使 用 语音 识别 功能 的 人 服务 。 多 信息 融合 反馈 的 人 机 交互 设 
备 在 市 场 上 尚 少 ,因此 ,基于 视 触觉 融合 的 虚拟 现实 人 机 交互 平台 有 很 好 的 前 景 。 

3. 人 机 交互 的 发 展 方向 

1) 高 科技 化 

计算 机 往 袖珍 化 、 便 于 携带 、 功 能 更 加 强大 等 方向 发 展 ; 输入 方式 更 多 元 化 ,触摸 感应 、 电 
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光 笔 、 脑 电感 应 等 方式 将 逐渐 出 现在 现实 生活 中 ; 输出 方式 未 来 将 以 三 维 显 示 为 主 ,配合 其 他 
设备 营造 出 一 个 通 真 的 虚拟 环境 ,使 人 们 能 参与 其 中 ， 

2) 自然 化 

对 图 像 进行 修改 、 加 工 等 软件 技术 的 出 现 、 窗 口 界面 的 完善 、 人 工 智 能 的 发 展 ， ,使 得 VR 的 
实现 存在 可 能 。 对 于 图 形 处 理 、 人 机 交互 心理 学 等 方面 的 研究 逐渐 深入 ,再 配合 一 些 硬件 手 
段 , 可 以 产生 让 人 分 辨 不 出 真 假 的 感觉 。 未 来 ,人 机 交互 将 朝向 人 体 自然 动作 的 方向 发 展 , 如 
机 械 臂 根据 人 的 动作 做 出 反应 。 通 过 一 些 智能 的 设计 ,充分 发 挥 整合 ,处理 .协调 的 效果 ,让 其 
自然 地 融入 到 虚拟 环境 中 ,让 用 户 能 在 虚拟 环境 中 得 到 愉悦 的 体验 。 在 一 些 界面 设计 处 理 方 
面 ,尽量 使 用 文字 、 图 像 、 光 、 声 音 等 形式 ; 在 显示 技术 方面 ,向 贴近 人 们 生活 的 方向 设计 ; 在 
交互 方面 ,使 用 人 们 现实 生活 中 的 常用 语言 进行 沟通 ,产生 逼真 的 虚拟 环境 。 

3) 人 性 化 

未 来 ,人 类 会 越 来 越 离 不 开机 器 ,人 机 交互 会 朝 着 人 与 机 器 相互 促进 的 方向 发 展 。 随 着 科 
学 技术 的 快速 发 展 , 人 类 对 知识 的 学 习 和 应 用 成 本 可 能 会 大 幅 增加 ,这 又 会 反 过 来 制约 科技 的 
进步 。 因 此 ,人 机 交互 将 向 着 更 适合 人 类 、 更 方便 人 类 使 用 的 方向 优化 发 展 ,从 而 从 各 方面 增 
强人 类 的 能 力 ,突破 人 类 生理 结构 带 来 的 限制 。 

4. 人 机 交互 的 相关 技术 、 

人 机 交互 主要 涉及 硬件 .软件 、 多 媒体 技术 方面 等 。 硬 件 方面 一 般 是 输入 设备 和 输出 设 
备 。 输 入 设备 主要 有 手写 笔 、 摄 像 机 、 麦 克 风 .扫描 仪 .触摸 屏 .键盘 .鼠标 等 ; 输出 设备 主要 有 
各 类 显示 器 ,如 绘图 仪 .液晶 屏幕 .3D 投影 仪 .多 媒体 音箱 等 。 软 件 方面 主要 是 对 本 和 
行 分 析 、 创 作 、 检 测 效果 以 及 人 机 互动 界面 等 。 对 特定 用 户 的 需求 进行 相应 的 分 析 , 得 出 一 
合适 的 方案 ,然后 根据 方案 设计 出 对 应 的 模型 ,根据 模型 来 进行 搭建 组 装 ,最 后 进行 测试 。 上 
据 用 户 的 信息 反馈 ,如 果 产 品 适合 市 场 ,就 可 以 适当 优化 ,降低 成 本 ,进行 量 产 ,产生 效益 。 多 
媒体 技术 方面 主要 包括 一 些 图 像 处 理 、 声 音 处 理 、 声 音 合成 .数据 传输 、 图 形 压缩 等 技术 。 通 过 
处 理 ,可 以 让 人 们 更 方便 地 进行 人 机 交互 。 

本 章 将 介绍 多 种 场景 下 (如 视 触 觉 融合 .生物 反馈 等 ) 机 器 人 控制 技术 在 人 机 交互 中 的 一 
些 具体 应 用 。 


10.1 基于 视 触觉 融合 的 虚拟 现实 人 机 交互 平台 


10.1.1 虚拟 现实 


1. 虚拟 现实 的 特征 

1994 年 ,Burdea 和 Coiffet 描述 了 虚拟 现实 (Virtual Reality， VR) 的 3 个 基本 入 寺 征 ,分别 
是 交互 (Interaction)、 沉浸 (Immersion) 和 想象 (Imagination) 。 由 于 这 3 个 英文 单词 都 是 以 
“I” 为 首 字母 ,因此 ,后 来 的 学 者 都 称 为 VR 的 3 个 “特征 。 

对 这 3 个 “I” 特 征 ,Burdea 和 Coiffet 给 出 以 下 解释 : VR 是 用 计算 机 图 形 来 构造 出 接近 真 
实 世 界 的 一 种 仿真 模拟 。 但 该 仿真 模拟 是 动态 的 ,可 以 对 用 户 的 输入 做 出 响应 ,这 就 是 实时 的 
交互 特征 一 一 VR 的 关键 特征 。 人 们 看 见 屏幕 上 的 影像 时 ,希望 影像 能 按 自己 的 指令 发 生变 
化 ,从 而 被 整个 虚拟 仿真 所 吸引 ,这 就 是 前 面 说 的 “沉浸 ?。 然 而 ,虚拟 现实 的 应 用 是 由 VR 开 
发 者 设计 的 ,应 用 的 各 种 功能 都 由 开发 者 决定 ,功能 的 优越 程度 取决 于 开发 者 的 想象 力 , 这 就 
荐 XR 的 第 三 不 下 一 想 尔 : 

“交互 ? 指 的 是 VR 系统 能 够 提供 基于 日 常 行 为 的 人 机 互动 ,用 户 可 以 很 方便 地 与 虚拟 影 
像 以 及 其 中 的 物体 进行 交互 感知 。“ 沉 浸 ” 又 称 为 “临场 感 ”, 指 用 户 有 接近 真实 的 体验 ,用 户 作 
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为 虚拟 现实 系统 的 主体 参与 其 中 。 为 了 实现 允 真 的 沉浸 感 ,需要 虚拟 现实 系统 提供 视觉 、 触 
觉 \ 听 觉 , 嗅 觉 及 味觉 在 内 的 所 有 感知 能 力 。“ 想 象 ” 指 用 户 必 须 拥有 非常 丰富 的 想象 力 。VR 
为 用 户 提供 了 非常 巨大 的 想象 空间 ,将 现实 世界 重新 展现 的 同时 ,还 能 够 创造 现实 世界 无 法 构 
建 出 来 的 虚拟 物体 ,丰富 了 人 类 的 认 知 界限 。 

2. 虚拟 现实 的 分 类 

VR 研究 目标 的 广阔 性 、 对 象 的 不 确定 性 和 应 用 需求 的 复杂 性 决定 了 其 应 用 于 学 科 综 合 
交织 穿插 、 系 统 类 别 多 样 的 科学 技术 领域 。 虚 拟 现 实 系 统 根据 不 同 的 定义 有 不 同 的 分 类 。 从 
应 用 的 定义 上 看 ,虚拟 现实 系统 可 以 分 为 规划 设计 、 展 示 娱 乐 及 训练 演练 等 几 类 。 从 沉浸 体验 
的 定义 上 看 ,VR 系统 有 群体 -VR 交互 式 体验 、 人 -VR 交互 式 体验 及 非 交 互 式 体 验 三 类 。 


10.1.2 基于 视 触 觉 融 合 的 虚拟 现实 人 机 交互 平台 设计 

1. 人 机 交互 平台 的 设计 思路 

该 人 机 交互 平台 的 具体 设计 思路 如 下 。 

(1) 视 触 觉 融 合 的 实现 。 要 将 视觉 和 触觉 融合 ,就 要 将 虚拟 影像 和 提供 触觉 的 被 操控 实 
物 物体 重合 ,因此 初步 拟定 需要 平台 实现 视觉 所 看 的 物体 影像 和 要 操控 的 物体 重合 的 功能 。 
初步 拟定 使 用 半 透 明 玻璃 板 将 影像 和 实物 物体 结合 起 来 。 

(2) 虚拟 现实 影像 的 实现 。 虚 拟 现实 技术 是 一 种 创建 虚构 的 世界 和 让 用 户 真实 体验 虚构 
的 世界 的 计算 机 仿真 系统 , 它 利用 计算 机 生成 一 种 虚拟 世界 ,是 一 种 多 信息 融合 的 交互 的 三 维 
动态 影像 的 系统 仿真 , 它 可 以 使 用 户 沉 浸 到 该 世界 中 。 为 达到 用 户 的 沉浸 感 强 的 效果 ,该 人 机 
交互 平台 需要 提供 逼真 的 虚拟 影像 ,这 就 要 求人 机 交互 平台 具有 电子 计算 机 和 显示 虚拟 影像 
的 三 维 显示 器 ,同时 ,还 需要 多 媒体 音箱 来 提高 虚拟 环境 的 沉浸 感 。 

(3) 触觉 反馈 设备 的 选取 。 触 觉 反馈 设备 应 该 具有 高 自由 度 ,触觉 反馈 效果 好 的 设备 , 才 
能 完美 地 模拟 出 虚拟 影像 的 力 效果 。 初 步 拟定 使 用 Touch X 机 器 人 。Touch X 是 一 个 6 自 
由 度 机 器 人 ,是 触摸 式 的 人 机 交互 的 力 反 馈 设备 。Touch X 具有 两 个 基本 的 功能 一 一 力 触 觉 
反馈 和 输入 位 姿 。 

(4) 平台 应 具有 调节 功能 。 每 个 人 的 体格 ,行为 举止 习惯 不 同 ,虚拟 的 影像 不 同 ,需要 的 
操作 空间 也 不 同 ,这 就 要 求 平台 对 于 不 同 的 用 户 和 不 同 的 虚拟 物体 能 够 做 出 不 同 的 调整 ,能 按 
照 用 户 的 具体 需要 进行 调整 。 因 此 , 半 透 明 玻璃 板 的 位 置 以 及 水 平 的 角度 应 该 是 可 调 的 ; 同 
样 地 ,3D 显示 器 相对 于 半 透 明 玻 璃 板 的 位 置 也 应 该 是 可 调 的 。 这 样 , 设 计 半 透明 玻璃 板 支架 
时 ,需要 考虑 加 入 高 度 调节 结构 和 旋转 调节 结构 。3D 显示 器 方面 初步 拟定 使 用 显示 器 支架 ， 
这 个 支架 要 求 具 有 调整 高 度 、 水 平角 度 以 及 显示 器 显示 的 功能 。 

(5) 人 机 交互 平台 应 该 具有 综合 性 。 人 机 交互 平台 最 后 应 该 能 结合 不 同 的 反馈 设备 , 例 
如 可 以 结合 麦克 风 和 多 媒体 音响 达到 声音 反馈 ,或 者 结合 Leap Motion 建成 一 个 手势 控制 的 
虚拟 环境 。 还 可 以 加 入 脑 电 采 集 设备 或 者 肌 电 采集 设备 ,从 而 为 平台 提供 多 种 控制 策略 或 者 
检测 用 户 的 生理 信息 ,将 用 户 的 生理 状态 实时 地 反馈 给 用 户 。 

(6) 人 机 交互 的 空间 。 应 该 预 留 空间 用 于 放置 Touch X 力 反 馈 机 器 人 或 者 Leap Motion 
体感 控制 器 等 用 于 人 与 机 器 交流 的 设备 。 考 虑 到 Touch X 和 Leap Motion 的 操作 空间 大 小 ， 
预 留 一 个 50cm( 长 )X45cm( 高 )X30cm( 宽 ) 的 立体 空间 。 

(7) 建立 3D 模 型。 在 搭建 实物 前 ,应 该 先 设 计 出 基于 视 触 觉 融合 的 虚拟 现实 人 机 交互 

台 的 3D 模型 。 所 有 的 调整 修改 方案 对 比 ,都 应 该 在 3D 模型 上 进行 ,直至 得 出 最 终 版 
本 ,经 过 各 种 方法 证 实 可 行 , 才 进行 实物 搭建 。 而 3D 模型 使 用 Solidworks 软件 进行 设计 与 
绘制 。 
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(8) 人 机 交互 平台 实物 的 搭建 。 根 据 人 机 交互 平台 的 3D 模型 ,确定 实物 搭建 所 需要 的 材 
料 与 器 材 , 同 时 罗列 出 实物 搭建 的 步骤 与 要 注意 的 地 方 , 然 后 根据 罗列 的 步 又 和 3D 模型 ,一 
步 一 步 地 将 实物 搭建 出 来 。 4 

(9) 人 机 交互 平台 的 调试 与 优化 。 在 人 机 交互 平台 实物 搭建 出 来 后 ,对 平台 的 基本 功能 
进行 测试 ,例如 测试 平台 的 虚拟 影像 投影 效果 如 何 , 测 试 反馈 设备 的 控制 效果 如 何 。 同 时 ,还 
需要 测试 平台 的 质量 ,看 看 实物 与 3D 模型 的 区 别 , 有 没有 需要 修改 的 地 方 , 若 有 , 则 进行 最 后 
的 修改 与 调整 。 当 基本 测试 完成 后 , 则 考虑 平台 的 优化 方向 ,罗列 出 平台 能 优化 的 地 方 ,得 知 
哪些 是 现在 可 以 实现 的 ,哪些 是 未 来 可 以 实现 的 。 

2. 人 机 交互 平台 设计 

结合 设计 时 的 思路 ,初步 拟定 基于 视 触觉 融合 的 虚拟 现实 人 机 交互 平台 方案 。 基 于 视 触 
觉 融合 的 虚拟 现实 人 机 交互 平台 如 图 10. 1 所 示 。 玻 璃 板 支 架 由 型 材 及 其 部 件 搭建 ,而 半 透 明 
玻璃 板 则 由 半 透 膜 贴纸 和 玻璃 板 组 成 。 


(1): 显示 器 支架 ; (2): 3D 显 示 器 ; 

(3): 显示 器 支架 杠 ; (4) : 半 透 明 玻璃 板 ; 
(5) : 玻璃 板 支架 ; (6) : 支撑 平台 ; 

(7): 多 媒体 音箱 ; (8) : Touch X 机 器 人 ; 
(9): 电子 计算 机 主机 


图 10.1 基于 视 触 觉 融合 的 虚拟 现实 人 机 交互 平台 


玻璃 板 支架 使 用 型 材 搭 建 , 结 构 分 别 如 图 10. 2 和 图 10. 3 所 示 。 由 一 根 al 和 两 根 a3 组 
成 玻璃 板 支架 的 固定 轴 和 承重 轴 , 使 用 b3 和 b4 将 a3 固定 在 支撑 平台 ,同时 使 用 b3 和 b4 来 
调整 al 的 高 度 , 从 而 达到 调整 半 透 明 玻璃 板 的 高 度 的 效果 。 由 一 根 a2 和 两 根 a4 组 成 玻璃 板 - 
的 固定 框架 。 通 过 b2 和 b3 来 固定 a2 与 a4 的 连接 。 使 用 型 材 自 带 的 凹 槽 放置 半 透 明 玻璃 
板 。 通 过 bl 的 使 用 ,将 玻璃 板 框架 与 固定 轴 连 接 起 来 ,同时 通过 调整 bl 的 水 平角 度 , 从 而 调 
整 半 透 明 玻璃 板 的 水 平角 度 。 通 过 调整 显示 器 支架 来 调整 3D 显示 器 的 水 平角 度 , 以 及 通过 
调整 显示 器 支架 杠 的 长 度 来 调整 3D 显示 器 的 高 度 。 

该 人 机 交互 平台 的 特征 是 : 通过 电子 计算 机 模拟 出 虚拟 影像 ,由 3D 显示 器 显示 ,然后 通 
过 半 透 明 玻璃 板 的 反射 成 像 原理 ,通过 调整 3D 显示 器 和 半 透 明 玻 璃 板 的 相对 位 置 ,将 虚拟 影 
像 呈 现在 玻璃 板 下 方 ,与 Touch X 机 器 人 重合 。 用 户 在 操作 Touch X 机 器 人 的 同时 ,通过 机 
器 人 的 力 反馈 信息 ,让 对 应 的 虚拟 影像 做 出 相同 的 动作 ,让 用 户 感觉 自己 在 操作 实物 。 同 时 ， 
该 系统 用 多 媒体 音箱 来 提高 用 户 在 虚拟 环境 的 沉浸 感 。 
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图 10.2 玻璃 板 支架 结构 图 1 图 10.3 玻璃 板 支 架 结构 图 2 


3. 基于 视 触觉 融合 的 虚拟 现实 人 机 交互 平台 的 系统 组 成 

该 基于 视 触 觉 融合 的 虚拟 现实 人 机 交互 平台 可 以 分 为 以 下 两 个 子 系统 。 

1) 3D 视觉 系统 

3D 视觉 系统 由 3D 显示 器 、 显 示 器 支架 .显示 器 支架 杠 以 及 半 透 明 玻璃 板 组 成 。 通 过 3D 
显示 器 呈现 出 虚拟 影像 ,基于 半 透 明 玻 璃 板 的 影像 反射 原理 ,用 户 可 以 看 到 虚拟 影像 ,将 虚拟 
影像 放置 在 半 透 明 玻璃 板 下 方 ,与 力 触觉 反馈 设备 重合 ,将 成 功 实现 视觉 与 触觉 的 完美 融合 。 

2) 力 反馈 系统 

力 反馈 系统 是 由 Touch X 机 器 人 等 力 反 馈 设备 ,电子 计算 机 主机 及 其 对 应 的 软件 组 成 。 
力 反 馈 系统 采集 用 户 操 作 机 械 臂 的 力 信 息 ,将 其 反馈 给 电子 计算 机 主机 ,电子 计算 机 主机 将 做 
出 对 应 的 运算 ,最 后 将 处 理 后 的 力 反馈 信息 反馈 给 虚拟 影像 。 最 终 呈现 出 来 的 效果 是 用 户 感 
觉 在 操控 虚拟 影像 对 应 的 实物 。 

4. 基于 视 触 觉 融 合 的 虚拟 现实 人 机 交互 平台 的 优点 

目前 ,人 机 交互 的 产品 日 渐 增多 。 但 大 部 分 产品 都 只 注重 视觉 反馈 ,对 于 视觉 反馈 的 研究 
较 深 , 而 对 力 触觉 反馈 的 研究 还 不 足 。 而 力 触觉 是 唯一 一 个 可 以 在 接收 用 户 和 周围 环境 的 输 
入 的 同时 ,又 可 以 对 用 户 和 周围 环境 做 出 输出 的 感知 通道 。 因 此 , 力 觉 /触觉 反馈 在 虚拟 环境 
中 具有 绝对 的 突出 优势 , 力 触觉 反馈 使 虚拟 现实 影像 变 得 更 加 逼真 ,加 强 了 虚拟 现实 的 沉 
浸 感 。 

该 人 机 交互 平台 可 以 调整 显示 器 的 高 度 位 置 和 水 平角 度 , 同 时 可 以 调整 玻璃 板 的 位 置 ,从 
而 使 虚拟 影像 和 力 反 馈 设备 完美 重复 ,用 户 操 控 Touch X 设备 时 ,具有 6 自由 度 的 Touch X 
可 以 让 用 户 自由 地 操作 ,计算 机 收 到 Touch X 的 输入 信息 后 ,实时 地 对 3D 影像 做 出 与 用 户 操 
作 同 样 的 操作 ,实时 显示 变化 的 3D 影像 。 同 时 ,Touch X 接收 到 计算 机 反馈 的 信息 ,将 力 信 
息 反 馈 给 用 户 ,用 户 收 到 触觉 和 视觉 上 的 反馈 后 ,又 对 Touch X 做 出 新 的 操作 …… 如 此 反复 ， 
使 得 用 户 可 以 操控 “影像 ”, 达 到 了 视 触 觉 的 完美 融合 ,实现 了 视觉 .听觉 .触觉 的 三 维 同 步 泻 染 
及 配合 ,具有 非常 强 的 沉浸 感 , 让 用 户 形成 对 虚拟 模型 的 一 个 完整 的 .正确 的 、 盘 真 的 认识 。 同 
时 ,该 平台 支持 综合 拓展 ,可 以 加 入 脑 电 采集 设备 `. 肌 电 采 集 设备 等 来 满足 用 户 的 各 种 需求 ; 
还 可 以 改变 丸 反 馈 设 备 的 种 类 ,例如 换 成 Leap Motion 体感 控制 器 来 使 用 手势 操作 等 。 


10.1.3 基于 视 触觉 融合 的 虚拟 现实 人 机 交互 平台 的 搭建 
1. 人 机 交互 平台 搭建 前 的 准备 
首先 ,确定 3D 显示 器 的 型 号 。3D 技术 分 为 不 闪 式 3D 技术 ,快门 式 3D 技术 ,裸眼 式 3D 
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技术 共 3 类 。 本 设计 使 用 华硕 的 VG248QE 显示 器 。VG248QE 显示 器 使 用 快门 式 3D 技术 ， 
所 以 需要 搭配 NVIDIA 3D Vision 2 整套 设备 才能 达到 3D 显示 效果 。 华 硕 VG248QE 显示 器 
屏幕 长 54cm, 宽 30cm, 由 此 可 以 确定 半 透 明 玻 璃 板 的 初步 规格 。 

其 次 ,确定 显示 器 支架 型 号 。 显 示 器 支架 使 显示 器 位 置 在 高 度 上 可 以 调节 ， 同时 ， 可 以 在 
水 平角 度 上 调节 。 为 了 达到 这 种 调节 效果 ,本 设计 采用 支 尔 成 电脑 显示 器 支架 。 该 款 显示 器 
支架 高 度 调节 有 两 段 式 ,第 一 段 可 以 达到 80cm 可 调 , 臂 长 达到 55cm, 显 示 器 可 以 向 上 翻转 
90" ,向 下 翻转 85° ,横竖 屏 自 由 360 "旋转 。 该 款 显 示 器 支架 的 性 能 符合 本 设计 。 

然后 ,确定 型 材 型 号 .规格 以 及 长 度 。 本 设计 采取 铝 合 金 方 管 框架 型 材 4040,al 为 30cm 
(1 根 ) 、a2 为 30cm(1 根 ) .a3 为 45cm(2 根 ) 、a4 为 50cm(2 根 )。 角 件 4 套 ,转向 角 件 1 套 ,L 
型 连接 板 2 套 。 

选取 玻璃 板 时 ,考虑 到 基于 视 触 觉 融合 的 虚拟 现实 人 机 交互 平台 需要 经 常 根据 用 户 需 要 
而 使 用 透明 度 不 一 样 的 玻璃 板 ,这 就 要 求 玻璃 板 轻便 、 坚 硬 以 及 耐 磨损 。 综 上 考虑 ,本 设计 的 
玻璃 板 采 用 亚克力 板 。 亚 克 力 板 可 以 与 铝 型 材 完美 结合 ,而 且 透 光 性 好 、 比 玻璃 轻 而 且 不 易 破 
碎 ,本 平台 采用 的 亚克力 板 规格 参数 为 32cm X50cm。 半 透 膜 选 用 防 紫 外 线 玻璃 贴膜 ,规格 为 
32cm X 50cm。 

计算 机 主机 采用 IPC 型 工业 控制 计算 机 并 配备 键盘 鼠标 ,而 支撑 平台 使 用 折 又 式 办 公 
桌 ,其 规格 为 长 80cm、 宽 40cm、 高 75cm。 力 反馈 设备 采用 Touch X 机 器 人 ,并 使 用 Leap 
Motion 。 

2. 人 机 交互 平台 的 搭建 

由 于 玻璃 板 支架 需要 固定 在 支撑 平台 ,所 以 需要 先 确定 玻璃 板 支架 的 位 置 。 人 机 交互 平 
台 搭 建 的 步骤 如 下 。 

第 一 步 , 组 装 玻璃 板 支 架 。 首 先 , 组 装 玻璃 板 框架 。 使 用 世 型 连接 板 将 1 根 a2 和 两 根 a4 
组 装 成 U 型 玻璃 板 框架 ,然后 使 用 转向 角 件 将 U 型 玻璃 板 框架 的 a2 和 al 连接 起 来 。 再 通过 
角 件 将 两 根 a3 和 al 组 装 起 来 ,这 样 玻璃 板 支 架 就 成 功 组 装 好 。 

第 二 步 ,固定 玻璃 板 支架 。 将 第 一 步 组 装 好 的 玻璃 板 支架 和 支撑 平台 组 装 起 来 ,需要 用 到 
剩 下 的 两 套 角 件 , 角 件 b3 的 一 边 连接 a3 , 另 一 边 连接 支撑 平台 。 因 此 ,需要 先 在 支撑 平台 打 
两 个 孔 。 通 过 支撑 平台 上 的 孔 ,将 角 件 的 另 一 端 固定 在 支撑 平台 上 ,这样 , 就 可 以 将 玻璃 板 支 
架 固 定好 。 

第 三 步 , 组 装 显示 器 支架 。 首 先 ,将 支架 杠 固定 在 支撑 平台 。 固 定 的 方法 有 两 种 ,一 种 是 
夹 片 式 ; 另 一 种 是 通过 螺钉 固定 。 本 设计 采取 来 片 式 固 定 ,以 方便 调整 显示 器 位 置 。 其 次 ,将 
3D 显示 器 的 底座 拆除 ,然后 ,将 3D 显示 器 和 显示 器 支架 通过 支架 自 带 的 4 个 螺钉 及 螺钉 孔 连 - 
接 起 来 。 最 后 ,将 显示 器 支架 和 显示 器 支架 杠 组 合 起 来 。 

第 四 步 ,电子 计算 机 与 平台 组 合 。 通 过 前 面 三 步 ,人 机 交互 平台 基本 成 型 , 接 下 来 需要 将 
剩 下 的 部 件 组 合 起 来 。 首 先 将 电子 计算 机 显示 器 连接 好 ,其 次 是 键 鼠 配件 ,最 后 将 需要 用 到 的 
反馈 设备 (如 Leap Motion 等 ) 连 接 起 来 。 

第 五 步 ,将 半 透 膜 贴 到 玻璃 板 上 。 首 先 , 将 玻璃 板 的 一 面 清洁 干净 ,然后 ,将 半 透 膜 反 面 的 
保护 膜 撕 下 来 ,向 半 透 膜 和 玻璃 板 喷 水 雾 , 紧 接着 将 半 透 膜 对 准 玻璃 板 , 从 一 个 角 开 始 贴 ,直至 
整 片 半 透 膜 贴 合 在 玻璃 板 上 。 最 后 ,使 用 塑料 刮 片 将 半 透 膜 里 的 气泡 和 水 泡 挤 走 。 

第 六 步 , 将 半 透 明 玻 璃 板 组 装 到 玻璃 板 支架 上 。 由 第 一 、 二 步 搭建 出 来 的 玻璃 板 支架 可 以 
看 出 ,玻璃 板 支 架 具有 一 个 开口 端 。 由 a2 和 a4 组 成 的 U 型 框架 具有 一 个 U 型 凹 槽 , 半 透 明 
玻璃 板 可 以 从 开口 端 进入 玻璃 板 支 架 ,完美 镶 舱 在 U 型 凹 槽 内 。 
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经 过 上 面 6 个 步骤 ,就 可 以 将 基于 视 触 觉 融合 的 虚拟 现实 人 机 交互 平台 搭建 成 功 , 实 物 图 
如 图 10.4 所 示 。 

3. 人 机 交互 平台 的 调整 优化 

虽然 基于 视 触觉 融合 的 虚拟 现实 人 机 交互 平台 的 虚拟 现实 沉浸 感 强 , 用 户 体验 好 ,能 支持 
其 他 功能 设备 加 入 ,但 本 设计 还 存在 可 以 优化 的 缺点 ,由 图 10.4 可 以 看 出 。 例 如 ,改变 玻璃 板 
支架 的 结构 ,将 玻璃 板 支 架 改 成 对 称 结构 ,这样 可 以 修正 图 中 玻璃 板 倾斜 的 问题 ; 还 有 可 以 增 
加 支撑 平台 的 面积 ,这 样 支撑 平台 有 更 多 的 面积 添加 补充 其 他 反馈 设备 ; 还 可 以 添加 一 块 不 
透明 玻璃 板 , 以 便 用 户 在 操作 时 不 需要 看 到 自己 的 手 ; 同时 ,该 平台 未 来 的 方向 可 以 向 着 体积 
缩小 ,轻便 的 方向 优化 。 该 平台 虽然 功能 出 色 , 但 由 于 体积 过 大 ,导致 只 能 将 其 固定 在 一 个 地 
_ 方 使 用 ,无 法 像 手 机 那样 随身 携带 。 

为 了 将 玻璃 板 支 架 改 成 对 称 结构 ,需要 添加 新 的 型 材 , 需 要 另外 增加 一 根 al 一 根 a2 \ 两 
根 a3 ,还 需要 角 件 4 套 、 转 向 角 件 1 套 、L 型 板 2 套 。 成 品 图 如 图 10. 5 所 示 。 


图 10.4 基于 视 触觉 融合 的 虚拟 现实 图 10.5 基于 视 触 觉 融合 的 虚拟 现实 
人 机 交互 平台 实物 图 人 机 交互 平台 改良 版 成 品 图 


10.2 应 用 于 生物 反馈 的 人 机 交互 技术 研究 


10.2.1 脑 电 模式 识别 算法 研究 与 设计 

1. 运动 想象 中 的 ERS/ERD 现象 

运动 想象 中 的 ERS/ERD 现象 指 的 是 : 大 脑 皮层 不 同 的 生理 反应 会 使 得 脑 电 的 波形 发 生 
相应 的 变化 。 当 大 脑 皮层 的 某 块 区 域 受 到 刺激 (如 感觉 神经 信和 号、 肢体 运动 和 运动 想象 指令 )， 
在 该 区 域 进行 的 信息 加 工 和 处 理 将 使 得 脑 电波 某 些 频段 的 幅 值 减 小 或 阻 滞 , 这 种 生理 电 现 象 
称 为 事件 相关 去 同步 (Event Related Desynchronization ,ERD) 。 相 反 地 , 当 该 区 域 没 有 受到 
刺激 而 处 于 静 息 状态 时 , 某 些 频段 的 脑 电 信和 号 表现 出 振幅 明显 增 大 的 现象 , 称 为 事件 相关 同步 
(Event Related Synchronization,ERS) 。 研 究 表明 ,ERD/VERS 现象 不 但 发 生 在 单 侧 肢体 活动 
中 ,而 且 在 单纯 的 运动 想象 中 也 会 产生 。 因 此 , 当 进 行 左手 的 运动 想象 时 ,大 脑 右 侧 的 脑 电 信 
号 的 功率 将 会 减 小 ,而 左 侧 的 脑 电 信号 的 功率 将 会 增 大 ; 同样 地 , 当 进 行 右手 的 运动 想象 时 ， 
大 脑 左 侧 的 脑 电功率 将 会 减少 ,而 右 侧 的 脑 电功率 将 会 增加 。 根 据 这 一 原理 ,可 以 将 运动 想象 
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的 脑 电 信号 分 成 两 类 一 一 想象 左手 运动 和 想象 右手 运动 。 

2. 数据 预 处 理 与 最 优 频 段 选 择 

研究 表明 ,ERS/ERD 现象 主要 体现 在 18 一 26Hz 和 8 一 12Hz 的 脑 电 频 带 上 。 但 对 于 不 
同人 ,ERS/ERD 现象 的 有 效 频 段 存在 差异 。 因 此 ,能 否 取 得 最 有 效 的 频带 对 脑 电 分 类 效果 有 
着 直接 的 影响 。 采 用 的 最 优 频带 选择 方法 如 图 10. 6 所 示 。 在 此 之 前 ,为 了 滤 除 噪声 信号 , 直 
接 在 脑 电 采 集 软件 SCAN4. 5 软件 上 完成 0.5~40Hz 带 通 滤波 。 然 后 ,将 4 一 40Hz 的 频段 分 
成 9 份 , 得 到 9 个 带 通 滤波 器 ,依次 为 4 一 8Hz,8~12Hz,…,36 一 40Hz。 滤 波 后 ,这 9 个 波段 
数据 将 被 放 入 特征 提取 模型 。 最 后 ,采用 互信 息 法 选择 能 够 较 好 地 区 分 ERS/ERD 现象 的 


最 优 特 征 


36~40Hz 
图 10.6 最 优 频 段 选择 方法 


3. 基于 CSP 的 特征 选择 方法 

在 基于 运动 想象 的 脑 电 分 类 识别 中 , 共 空 间 模 式 (Common Spatial Pattern,CSP) 算 法 的 
运用 非常 普遍 ,并 且 效 果 明 显 。 在 CSP 中 ,期望 的 空间 滤波 器 可 被 看 成 一 个 投影 矩阵 。 经 过 
该 矩阵 的 投影 ,可 以 将 一 类 信和 号 方差 最 大 化 的 同时 ,将 另 一 类 信和 号 的 方差 最 小 化 ,从 而 提取 出 
最 突出 的 ERD/ERS 特征 ,使 得 EEG 信号 在 投影 后 的 空间 可 以 最 好 地 区 分 开 。 

为 了 方便 分 析 , 假 设 单 次 实验 采集 的 脑 电信 和 号 经 过 滤波 等 预 处 理 后 为 Nc XT 的 二 维和 矩 
阵 ,其 中 Nc 表示 脑 电 采 集 的 通道 数 ,T 代表 每 个 通道 采集 的 时 间 长 度 , 即 数据 长 度 。 为 了 求 
取 最 优 的 空间 投影 矩阵 , 先 要 求 取 脑 电信 和 号 的 空间 协 方差 矩阵 : 

全 的 


ou Qe ie trCX XT) 人 
> ey (10. 2) 
| Qr |;€8, tr(X,X!) , 
其 中 ,XX;, ER “” 为 上 述 所 说 的 通道 数 为 Nc 数据 长 度 为 工 的 第 ; 组 经 过 预 处 理 的 脑 电 信 


号 ; tr() 是 矩阵 的 迹 ( 对 角 和 矩阵 的 元 素 之 和 ); QI 和 Qe 代表 运动 想象 脑 电 数据 的 两 种 分 


类 一 一 想象 左手 运动 和 想象 右手 运动 。 释 加 两 类 协 方差 ,有 : 
C=CL+Cr | (10. 3) 
通过 特征 分 解 可 以 得 到 ， 
C 一 WeRDC (10. 4) 


其 中 ,Uc 为 特征 向 量 和 矩阵 ,TT 是 特征 值 对 角 和 矩阵 。 于 是 可 以 求 出 白化 (whitening) 变 换 
PE (10. 5) 
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通过 白化 变换 ,可 以 将 Cl 和 CR 转化 成 : : 
下 =PXCF XP (10. 6) 


Tk=PXCr XP (10.7) 

并 且 Ti 和 Tr 具有 相同 的 特征 向 量 : 
yy (10. 8) 
Ti=B XA XB (10. 9) 

不 难 发 现 ,A， 和 AR 还 有 如 下 性 质 

Ai +Ax =1 (10. 10) 

因此 ,可 求 得 空间 投影 矩阵 ,如 下 所 示 
W=(P' XB)T (10. 11) 

空间 转换 矩阵 可 以 将 C1 和 CR 转换 成 以 下 形式 : 

Ai =WCi WwW! (10. 12) 
Ar =WCrW CLIO. 13 


其 中 ,A =diag(a! py ,Qt ) 和 AR =diag(a® 5 se ,a ) 是 特征 值 组 成 的 对 角 和 矩阵 ， 二 
足 aT 十 a*= 二 1。 这 意味 着 当 C1 的 特征 值 wz 最 大 时 ,CR 的 特征 值 a* 为 最 小 。 这 里 将 a™ 
行 降序 排列 ,投影 矩阵 也 跟随 变化 ,然后 选择 投影 矩阵 中 的 最 顶端 和 最 底 端 的 / i ot 
投影 矩阵 妈 。 最 后 ,可 以 提取 出 具有 最 佳 区 分 效果 的 协 方差 特征 : 
diag (WX,X!W) 
f ep li) =log (10. 14) 
> aiagtwx, XIW)); 

4. 基于 LDA 的 分 类 算法 

线性 判别 式 分 析 (Linear Discriminant Analysis,LDA) 也 叫 作 Fisher 线性 判别 (Fisher 
Linear Discriminant, FLD) , 它 是 在 1996 年 由 Belhumeur 提出 并 应 用 到 图 像 处 理 中 , 现 已 成 为 
模式 识别 和 人 工 智能 领域 的 经 典 算法 。LDA 算法 的 基本 思想 是 找到 一 个 最 佳 的 投影 方式 ( 即 
投影 矩阵 ) ,把 原来 高 维 数据 样本 投影 到 一 个 低 维 空 间 中 ,使 得 投影 后 不 同类 别 的 数据 样本 尽 
可 能 地 分 开 ( 距 离 最 大 ) ,而 同一 类 别 的 数据 样本 尽 可 能 地 聚集 (离散 度 最 小 ) ,使 不 同类 别 的 数 
据 样 本 达到 最 好 的 分 离 效 果 。 

假定 x'€ {zxi,zs,…,X%),i€ (1,2) 为 两 类 运动 想象 脑 电 信号 经 过 CSP 特征 提取 后 的 数 
据 样 本 。 如 前 所 说 , 它 可 以 由 投影 矩阵 投影 到 一 个 低 维 空间 中 ,如 >=vz。 投 影 前 后 ,两 类 
特征 样本 的 均值 可 以 表示 为 : 


Ai 一 一 处 直人 15) 
人 
pi = > Ul (10. 16) 
FE 
其 中 ,NN, 为 i 类 样本 数量 。 投 影 前 后 ,类 内 散布 矩阵 可 以 表示 为 : 
2 
= 二 二 这 (10. 17) 
i=lzr€rx! 
2 
= >) (vr) =v Sv (10. 18) 


i=lr€érx' 
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同样 地 ,可 得 类 间 散 布 矩阵 : 
S$, = 一 pa)(CA 一 Ap) (10. 19) 
§, = (1 pp p20) =v! Sv ` (10.20) 


如 上 所 述 , 如 果 分 类 样本 具有 最 大 的 类 间距 离 和 最 小 的 类 内 距离 ,其 分 类 结果 将 会 达到 最 
好 。 在 这 里 采用 Fisher 判别 准则 : S 


二 (10.21) 


只 需要 提取 J (最 大 值 时 对 应 的 投影 矩阵 mw ,就 和 g 使 得 影 后 的 两 类 样本 的 分 离 效 果 最 好 。 
根据 拉 格 朗 日 乘 子 法 ,vw 可 以 由 下 式 求 得 ， 


S,v =AS,v (10. 22) 
可 见 , 最 佳 投影 矩阵 w 为 S'S 的 特征 向 量 。 结 合 以 上 公式 ,可 以 求 得 : 
v=S, (uC—,) (10. 23) 


至 此 ,一 个 基于 x'€{xi,x，,…,x'},iE€1{1,2} 样 本 的 分 类 器 已 经 构建 完毕 ,新 的 样本 数 
据 可 以 通过 这 个 分 类 准则 进行 分 类 。 分 类 准则 可 以 表示 为 : 
oy(z)=v z+ vw, (10. 24) 
其 中 ,z 为 新 的 样本 数据 ,o 为 LDA 分 类 器 的 投影 矩阵 ,zu 为 偏 移 量 。 当 运算 结果 y(z) 二 0 
时 ,意味 着 样本 xz 属于 一 类 信号 ; 而 当 y(z)<0 时 , 则 意味 着 属于 另 一 类 。 


10.2.2 游戏 设计 与 实现 
1. 游戏 整体 布局 与 设计 


如 图 10.7 所 示 ,游戏 界面 一 共 由 3 个 部 分 组 成 : 游戏 窗口 ,波形 显示 和 用 户 界面 。 接 下 
来 依次 对 这 3 个 部 分 进行 介绍 。 


LE 
bd 有 


图 10.7 游戏 窗口 
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在 如 图 10.7 所 示 的 游戏 窗口 中 ,将 一 个 星球 放 在 一 个 看 似 飞行 的 平台 上 。 脑 电信 和 号 经 过 
处 理 分 类 成 两 类 信和 号 ,用 以 控制 平台 顺 时 针 或 者 逆 时 针 运 动 。 如 果 平 台 不 是 水 平 的 ,由 于 受到 
重力 和 支持 力 的 相互 作用 ,星球 将 朝 着 平台 较 低 的 一 端 滚动 ,如 果 平 台 不 进行 旋转 调整 ,星球 
将 滚 出 平台 落 入 星空 (假设 星球 在 星空 中 受到 垂直 向 下 的 重力 或 引力 ) ,效果 如 图 10.8 所 示 。 
为 了 避免 星球 落 入 星空 ,平台 需要 通过 脑 电 不 断 进行 调整 以 维持 相对 平衡 ,让 星球 位 置 稳定 在 
平台 上 。 星 球 在 平台 上 的 时 间 越 长 ,游戏 分 数 越 高 ,一 旦 星球 落 入 星空 ,游戏 就 结束 。 为 了 增 
加 游戏 的 乐趣 ,设置 了 3 个 过 程 进入 新 的 等 级 之 前 ,平台 会 闪烁 
2s 提示 游戏 者 做 好 准备 。 一 旦 进入 更 高 级 别 ,平台 的 长 度 和 表面 摩擦 力 将 会 变 成 原来 的 0.8 
浊 ,但 是 游戏 者 可 以 在 相同 的 时 间 内 得 到 双 倍 的 分 数 。 游 戏 升级 依靠 游戏 者 的 表现 ,表现 的 评 
_ 佑 由 下 述评 价 函 数 即 式 (10. 25) 求 得 。 通 常情 况 下 ,游戏 者 的 注意 力 状态 越 好 ,表现 也 越 佳 , 因 
此 越 容易 进入 更 高 的 级 别 并 收获 更 多 的 分 数 。 为 了 更 好 地 呈现 游戏 者 的 状态 ,在 游戏 窗口 的 
右上 方 增加 了 一 个 卡通 脸 。 根 据 对 游戏 者 状态 的 评估 ,卡通 脸 会 表现 出 : 喜悦 .微笑 .平静 、 情 
发 、 悲 伤 ,如 图 10. 9 所 示 。 


逆 时 针 旋 转 平台 


时 间 /s 


顺 时 针 旋 转 平台 
图 10.8 平台 和 星球 的 运动 


微笑 平静 收发 悲伤 
图 10.9 卡通 脸 状 态 评估 


生物 反馈 需要 一 个 界面 去 显示 人 的 生理 状态 或 心理 状态 ,人 再 根据 情况 进行 调整 。 如 果 
仅仅 依靠 卡通 脸 的 显示 ,虽然 很 形象 生动 ,但 是 不 能 精确 反映 游戏 者 的 状态 和 更 多 的 细节 , 因 
此 增加 了 波形 显示 模块 。 从 图 10.7 右 下 方 的 复 选 框 可 见 , 这 里 有 4 个 波形 可 供 选 择 , 包 括 平 
全 的 偏转 角度 、 星 球 的 横向 速度 、 加 速度 以 及 一 个 综合 评价 函数 。 这 些 选项 都 能 够 通过 某 项 指 
标的 效果 侧面 反映 游戏 者 的 注意 力 状 态 。 例 如 ,如 果 星 球 的 位 置 越 接 近 平 台 的 中 点 ,表示 星球 
较为 安全 地 竺 在 平台 上 不 至 于 落 入 星空 ,因此 游戏 的 控制 效果 就 越 好 。 对 于 所 有 的 波形 值 也 
这 样 , 越 接近 0 的 时 候 , 游 戏 的 控制 效果 越 好 。 但 是 ,正如 上 面 的 例子 ,如 果 星 球 的 位 置 非常 接 
近 中 点 ,但 是 速度 很 大 ,星球 依然 很 有 可 能 在 惯性 的 作用 下 冲 出 平台 。 因 此 ,单一 的 评价 指标 
不 足以 评估 游戏 的 控制 效果 ,这 里 需要 一 个 综合 ,全 面 的 评价 指标 : 


ES 


| wa ts | 工 | (10. 25) 
Ve 


其 中 ,v(t) 和 x 是 星球 的 速度 和 位 置 ; te 是 初始 时 刻 , 是 过 去 的 某 个 时 间 点 ; ty 为 终点 时 刻 ， 
也 就 是 当前 时 刻 ; a 和 6 是 权重 系数 。 综 合 评价 指标 跟 星球 的 位 置 和 速度 有 关 , 如 果 星 球 位 置 
接近 中 点 位 置 , 且 在 过 去 一 段 时 间 内 移动 都 很 缓慢 ,此 时 五 的 值 很 小 ,可 以 认为 游戏 控制 得 很 
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好 。 由 于 惯性 作用 使 得 高 速 运动 时 的 星球 难以 停 下 来 ,因此 采用 v(1)” 而 不 是 v 来 避免 速度 
过 大 。 在 游戏 中 ,如 果 游 戏 者 注意 力 不 集 中 ,游戏 控制 得 很 差 ,E 的 值 很 大 ,卡通 脸 会 变 得 翌 帐 
和 悲伤 起 来 。 相 反 地 ,如 果 游 戏 控制 得 很 好 ,E 的 值 很 小 ,卡通 脸 会 展现 出 微笑 甚至 喜悦 。 一 
旦 EE<é(& 是 开发 者 设置 的 一 个 很 小 的 数 ) ,游戏 将 会 进入 更 高 级 别 ( 由 “简单 ”升级 为 “一 般 ”， 

或 由 “一 般 ” 升 级 为 “困难 ”)。 

本 节 将 设计 一 款 生 物 反馈 游戏 ,使 得 使 用 者 在 玩 游戏 的 过 程 中 ,不 仅 可 以 享受 游戏 的 乐 
趣 , 更 重要 的 是 可 以 通过 游戏 放松 与 调整 自己 的 精神 状态 和 注意 力 水 平 。 对 于 游戏 设 定 ,不 同 
的 人 会 有 不 同 的 喜好 ,所 以 个 性 化 设计 变 得 非常 重要 。 如 图 10. 10 所 示 , 在 用 户 界面 中 ,游戏 
者 可 以 选择 他 们 喜爱 的 背景 和 音乐 。 为 游戏 者 提供 两 个 3D 效果 的 背景 界面 ,一 个 是 “璀璨 星 
空 ”; 另 一 个 是 “古老 城堡 ”。 选 择 自 己 喜 欢 的 背景 音乐 类 型 之 后 ,会 在 游戏 中 进行 切换 。 另 
外 ,游戏 者 通过 在 “g”“u”“R” 和 “L” 的 输入 框 中 输入 数值 ,就 可 以 任意 修改 游戏 难度 。 其 中 ， 
“g” 代 表 重 力 加 速度 ,“u” 代 表 摩 擦 力 系数 ,“R” 代 表 平 台 旋转 速度 ,“L” 代 表 平 台 的 长 度 。 如 果 
游戏 开始 前 没有 输入 数值 ,游戏 将 会 在 默认 参数 下 运行 。 游 戏 还 可 以 增加 用 户 名 ,用 以 保存 最 
高 分 .游戏 设 定 以 及 上 一 次 的 显示 波形 数据 。 当 使 用 者 下 次 玩 游戏 时 ,只 需要 输入 自己 的 用 户 
名 , 单 击 “Load” 按 钮 ,所 有 的 选项 和 游戏 信息 将 会 设置 成 和 上 次 登录 的 信息 一 致 ,并 且 可 以 看 
到 之 前 的 波形 数据 。 


丙 珠 星空 i 古老 城堡 
图 10.10 游戏 背景 界面 


2. 基本 游戏 机 制 的 实现 

如 图 10.7 所 示 的 游戏 窗口 是 QtCreator 提供 的 一 个 视图 (graphic view) ,与 其 他 控件 (如 
标签 .选项 框 、 按 钮 等 ) 一 样 ,视图 也 是 QtCreator 程序 主 窗口 的 子 类 。 程 序 主 窗口 中 的 所 有 子 
对 象 都 可 以 通过 信和 号 与 槽 的 机 制 进行 连接 。 在 游戏 窗口 虫 ,星球 .平台 卡通 脸 ,以 及 显示 分 数 
的 文本 和 显示 级 别 的 文本 都 是 QtCreator 提供 的 图 形 项 。 因 为 星球 和 平台 的 移动 既 有 相对 运 
动 , 同 时 也 存在 相互 接触 和 旋转 的 关系 。 eh 
同一 个 父 对 象 中 ,使 得 它们 具有 相同 的 坐标 系 。 于 是 ,只 要 旋转 父 对 象 ,星球 和 平台 也 会 以 父 
对 象 的 某 一 个 中 心 点 为 圆心 旋转 起 来 。 因 此 ,如 图 10. 11 A X 
轴 和 YY 轴 运 动 ,通过 操作 函数 “moveby(dx,dy)” 就 可 以 实现 。 另 外 ,为 了 让 星球 运动 时 看 起 来 
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= 


是 滚动 的 ,星球 需要 根据 移动 速度 进行 旋转 ,采用 “rotate(angle) ”函数 便 可 实现 。 


i 

/星球 : 人 
/moveby(dx=10,\ 相 计 
py dy=0); E / 1 


1 
\ 1 
SN qd=0 | ~ ; po * rotate(angle=90) ; ~、 GE d=0 上 1 


图 10.11 游戏 模型 的 运动 分 解 


星球 的 运动 基于 牛顿 第 二 定律 。 首 先 , 使 能 一 个 0. 02s 的 定时 器 。 每 0. 02s, 平 台 旋转 一 
个 确定 的 角度 ,通过 每 一 次 累加 可 以 得 到 平台 的 实际 角度 。 然 后 根据 平台 的 倾斜 角 可 以 求 出 
星球 沿 着 X 轴 和 YY 轴 的 位 移 和 加 速度 。 在 星球 处 于 平台 时 ,重力 只 有 沿 X 轴 的 分 量 ,小 球 沿 
X 轴 的 加 速度 和 位 移 可 以 表示 为 : 
av =g X sina (10. 26) 
d, =v,t 二 0.5at’ (10. 27) 
其 中 ,a 是 平台 的 实际 角度 ,g 是 重力 加 速度 ,t 是 0. 02s 的 定时 器 时 间 间 隔 ,v, 为 该 时 间 间 隔 
内 的 初速 度 ,可 以 由 下 式 求 得 : 


vr =V, 二 at (10. 28) 
其 中 ,zx 为 上 一 时 间 间 隔 的 初速 度 。 于 是 ,星球 滚动 的 旋转 角度 也 可 以 计算 出 来 : 
1 Ce xX ~ (10. 29) 
angje 一 开 DE , 


其 中 ,R 为 星球 的 半径 。 如 果 星 球 深 出 平台 , 它 将 会 有 沿 Y 轴 方 向 的 运动 。 
星球 沿 着 Y 轴 的 运动 可 以 用 下 式 表示 : 


ay 一 有 X cosa (10. 30) 
d, =v,t+0. 5at (10. 31) 
i i (10>32) 


求 出 星球 在 一 个 时 间 间 隔 (0. 02s) 的 位 移 和 旋转 角度 后 ,通过 QtCreator 接口 函数 
moveby(d, ,d , ) 和 rotate(angle) 便 可 实现 它 的 移动 和 旋转 。 由 于 时 间 间 隔 很 短 , 因 此 行星 的 
运动 看 起 来 是 连续 的 。 游 戏 的 得 分 也 是 每 间隔 0. 02s 更 新 一 次 。 分 数 由 下 式 得 到 : 

score 一 Score 十 2 一 Xt 《10. 33» 
其 中 ,scare 为 上 一 次 的 分 数 ; t 为 定时 器 时 间 间 隔 ,t 二 0.02s; ! 是 与 游戏 等 级 相关 的 系数 , 例 
如 令 简 单 级 别 的 /==1, 令 一 般 级 别 的 1 二 2。 这 样 ,级 别 越 高 ,分 数 就 可 以 越 快 拿 到 。 

3. 波形 显示 窗口 的 实现 

在 波形 显示 的 实现 中 ,采用 QtCreator 提供 的 一 个 非常 实用 的 数据 显示 库 QWT。 如 
图 10. 12 所 示 ,QWT 是 一 个 波形 显示 窗口 ,是 一 个 名 为 QwtPlot 的 Qt 类 。 在 波形 显示 之 前 ， 
首先 申请 4 个 QwtPlotCurve 对 象 代表 上 面 提 到 的 4 条 波形 曲线 ,然后 将 它们 隶属 到 QwtPlot 
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对 象 中 。 于 是 ,只 需要 给 每 个 QwtPlotCurve 对 象 设 置 要 显示 的 曲线 的 样本 数据 ,对 应 的 波形 
将 会 在 QWT 窗口 中 显示 出 来 。 因 此 ,正确 采集 需要 显示 的 曲线 数据 是 实现 波形 显示 功能 的 
关键 。 在 这 里 ,采用 QVector 容器 进行 数据 保存 ,每 次 计算 出 平台 的 偏转 角度 .星球 的 位 置 和 
速度 ,以 及 综合 评价 指标 ,需要 将 这 些 数据 通过 入 栈 的 方式 保存 到 QVector 容器 中 , 供 波 形 显 
示 使 用 。 另 外 ,显示 窗口 的 右 方 有 4 个 复 选 框 ,以 供用 户 选择 想 要 显示 的 波形 图 。 为 了 实现 这 
一 功能 ,采用 信号 与 槽 机 制 ,将 复 选 框 和 QWT 对 象 关 联 起 来 。 每 次 勾 选 复 选 框 ,就 会 有 一 个 
信号 发 射出 来 ,然后 Qt 程序 会 跳 转 到 与 该 信号 关联 的 槽 函数 中 。 所 以 只 需要 在 权 函 数 中 检 
测 每 个 复 选 框 的 状态 ,然后 将 被 选中 的 波形 曲线 显示 出 来 ,并 且 将 没有 选中 的 波形 曲线 隐藏 起 
来 即 可 ,显示 和 隐藏 可 以 很 方便 地 通过 调用 setVisible(true/false) 实 现 。 通 过 上 述 方法 ,波形 
曲线 就 可 以 显示 出 来 ,如 图 10. 12 所 示 。 当 然 , 如 果 不 想 看 波形 图 ,可 以 不 勾 选 ,但 是 波形 显示 
窗口 会 影响 游戏 界面 的 整体 美观 ,因此 设置 了 一 个 Waveform 按钮 , 单 击 此 按钮 可 以 收 起 或 弹 
出 波形 显示 窗口 。 | 


图 10.12 波形 曲线 显示 


4. 个 性 化 设计 的 实现 

个 性 化 设计 是 指 游戏 的 设 定 可 以 根据 不 同 的 使 用 者 进行 调整 。 在 介绍 的 游戏 中 ,用 户 可 
以 选择 自己 喜欢 的 背景 图 和 音乐 ,并 且 背 景 音乐 会 在 游戏 过 程 中 进行 切换 ,“ 场 景 ”选项 框 用 来 
切换 游戏 背景 。 同 样 基于 信和 号 与 槽 机 制 ,一 旦 使 用 者 在 选项 框 中 进行 选择 ,就 会 有 信号 发 出 ， 
程序 跳 转 到 对 应 的 槽 函数 ,只 需 在 槽 函数 中 切换 游戏 场景 即 可 。 采 用 Qt 类 “phono” 来 实现 背 
景 音乐 的 播放 。 背 景 音乐 的 选择 同样 有 4 个 复 选 框 , 每 个 复 选 框 对 应 一 类 音乐 。 使 用 链表 来 
保存 相同 类 型 的 音乐 路 径 ,通过 移动 链表 指针 便 可 以 找到 下 一 个 音乐 路 径 完 成 背景 音乐 的 切 
换 。 如 果 复 选 框 中 有 两 类 音乐 被 选中 ,只 需 将 一 类 音乐 的 链表 追加 到 另 一 类 音乐 的 链表 上 , 即 . 
可 生成 新 的 链表 ,实现 两 类 歌曲 的 循环 切换 。 对 于 游戏 难度 设置 ,同样 采用 信号 与 槽 的 机 制 ， 
当 输入 数值 到 “g”u””“R2 和 ”“L2? 时 ,就 可 以 在 槽 函数 上 对 重力 加 速度 .摩擦 系数 、 旋 转角 速度 和 
平台 长 度 进行 修改 。 当 按 下 用 户 名 下 方 的 save( 保 存 ) 按 钮 时 ,所 有 这 些 参 数 和 用 户 的 选择 以 
及 波形 数据 将 会 保存 为 用 户 信息 。 一 个 名 为 QDataStream 的 Qt 类 可 以 用 来 实现 数据 保存 功 
能 , 它 的 优势 在 于 不 管 保 存 的 数据 多 么 复杂 ,都 会 分 解 成 二 进 制 信息 流 输出 到 文件 中 。 因 此 ， 
只 需要 将 需要 保存 的 数据 一 个 接 一 个 地 输出 到 文件 中 ,然后 , 当 按 下 Load( 加 载 ) 按 钮 时 ,就 可 
以 在 该 文件 中 获取 到 一 个 接 一 个 的 数据 ,顺序 跟 保 存 时 的 一 样 。 到 这 里 ,个 性 化 设计 已 经 完 
成 。 最 后 ,将 游戏 窗口 波形 显示 .用户 界面 和 用 户 数据 保存 文件 进行 连接 ,如 图 10. 13 所 示 ， 
即 可 实现 所 有 游戏 功能 。 

5. 脑 机 接口 与 游戏 的 连接 

生物 反馈 游戏 系统 结构 如 图 10. 14 所 示 。 其 中 ,Neuroscan 设备 和 SCAN4. 5 配套 ,用 来 
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波形 信息 


图 10.13 游戏 整体 连接 


采集 脑 电信 号 。 由 于 QtCreator 拥有 界面 设计 上 的 优势 ,被 用 来 进行 游戏 开发 。 在 脑 电 数据 
”处 理 算 法 方面 ,选择 使 用 Python 语言 ,因为 它 的 代码 简洁 .高效 ,并 且 具 有 丰富 的 机 器 学 习 库 
可 以 进行 调用 。 另 外 ,还 使 用 了 MATLAB, 它 负责 将 上 述 软件 连接 起 来 ,完成 通信 功能 。 所 
以 ,整个 游戏 系统 的 运行 过 程 为 : 脑 电信 号 经 Neuroscan 设备 采集 后 ,通过 有 线 的 方式 发 送 到 
SCAN 4.5 软 件 上 。MATLAB 采用 TCP 端口 与 SCAN 4. 5 连接 ,以 实时 获取 脑 电 数据 。 然 
后 MATLAB 调用 Python 程序 进行 数据 处 理 , 其 中 包括 10. 2. 1 节 介 绍 的 预 处 理 、 特 征 提 取 、 
特征 选择 和 模式 分 类 ,并 最 终 将 脑 电信 和 号 分 成 两 类 一 一 想象 左手 运动 和 想象 右手 运动 。 最 后 ， 
将 该 分 类 结果 作为 控制 信号 通过 UDP 接口 发 给 游戏 ,游戏 接收 到 信号 即 可 做 出 反应 ,控制 平 
台 运 动 。 


生物 反馈 游戏 系统 
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图 10. 14 ”生物 反馈 游戏 系统 结构 图 


10.2.3 实验 过 程 与 结果 分 析 

1. 离线 脑 机 接口 实验 

离线 试验 设置 : 采用 Neuroscan 设备 对 脑 电 原始 数据 进行 采集 , 脑 电 幅 电极 分 布 如 图 10. 15 
所 示 。 脑 电信 和 号 的 采样 速率 为 500Hz, 采 集 后 在 SCAN 4.5 软件 上 先 对 0.5 一 40Hz 频段 的 脑 
电信 号 进行 滤波 。 每 一 次 试验 进行 10s, 其 中 包括 2s 的 准备 时 间 、3s 的 运动 想象 时 间 以 及 5s 
的 休息 时 间 ,流程 如 图 10. 16 所 示 。 
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1 1 
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图 10.16 单 次 试验 流程 


整个 实验 数据 处 理 流程 如 图 10. 17 所 示 。 首 先 ,需要 进行 多 次 试验 采集 一 定数 量 的 脑 电 
数据 样本 进行 脑 机 接口 的 训练 和 测试 ,例如 采集 120 个 样本 进行 训练 ,采集 60 个 样本 用 以 测 
试 。 在 离线 训练 中 , 带 有 标记 的 训练 数据 需要 经 过 预 处 理 、 特 征 提 取 、 选 择 和 分 类 。 其 中 ,CSP 
和 LDA 是 有 监督 的 学 习 算 法 ,因此 CSP 特征 提取 旨 在 获取 一 个 期 望 的 空间 滤波 矩阵 W ,而 
LDA 分 类 算法 的 目的 在 于 获取 最 优 的 投影 矩阵 w 和 偏 移 量 v。。 一 旦 这 3 个 参数 确定 ,特征 提 
取 和 分 类 的 模型 就 构建 完毕 ,在 接 下 来 的 测试 数据 和 新 采集 的 在 线 脑 电 数据 就 可 以 直接 使 用 
该 模型 进行 特征 提取 和 分 类 。 最 后 将 测试 数据 的 分 类 结果 与 它 的 标记 进行 对 比 ,就 可 以 得 到 
分 类 的 准确 率 。 


图 10.17 实验 数据 处 理 流 程 图 


2. 实验 结果 

在 离线 测试 准确 率 的 实验 中 ,邀请 3 位 健康 的 男性 测试 者 。 

当 人 想象 单 侧 肢 体 运动 时 ,它们 的 大 脑 会 产生 ERD/ERS 现象 。 为 了 获得 这 一 现象 ,首先 
对 原始 脑 电 数据 进行 滤波 ,然后 进行 傅 里 叶 变换 ,将 这 一 现象 呈现 出 来 ,如 图 10. 18 所 示 ,其 中 
C3、C4 是 分 别 位 于 左 脑 和 右 脑 的 两 个 电极 。 

根据 上 述 数 据 处 理 算法 以 及 实验 设置 ,对 采集 到 的 脑 电 数据 进行 训练 和 测试 。 图 10. 19 
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表示 的 是 一 个 受 试 者 的 脑 电 分 类 结果 图 ,可 以 看 到 ,尽管 测试 数据 效果 相对 较 差 ,训练 数据 和 
测试 数据 中 的 两 类 脑 电 信号 都 有 较 好 的 区 分 。 而 分 类 准确 率 如 表 10. 1 所 示 ,平均 识别 准确 率 
接近 70% ,这 确保 了 游戏 控制 能 够 达到 较 好 的 效果 。 

训练 数据 测试 数据 


J = #0 0 0 EL =o sd RS 


图 10. 19 训练 和 测试 数据 的 分 类 结果 图 


表 10.1 脑 电 分 类 准确 率 % 


训练 数据 准确 率 测试 数据 准确 率 


测试 者 


Sl 89 70. 00 
S2 “88 68.75 
S3 | 91 | 66. 25 
平均 值 89. 33 68. 33 


经 过 离线 试验 的 训练 和 测试 后 ,特征 提取 ,选择 以 及 分 类 的 模型 已 经 成 功 构 建 ,因此 在 线 
脑 电 控制 可 以 直接 使 用 训练 好 的 模型 。 在 线 采 集 的 脑 电 数据 经 过 10. 2 节 所 述 的 数据 处 理 算 
法 后 ,得 到 脑 电 分 类 结果 ,其 作为 控制 信号 发 送 给 游戏 ,从 而 实现 游戏 控制 。 游 戏 者 只 需要 穿 
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戴 好 Neuroscan 脑 电 帽 , 通 过 想象 即 可 玩 游戏 ,如 图 10. 20 所 示 。 游 戏 中 , 当 星 球 问 平台 的 左 
端点 运动 时 ,为 了 防止 其 滚 出 平台 ,游戏 者 需 想象 左手 运动 以 使 平台 顺 时 针 旋 转 。 当 星球 向 平 
台 右 端 运动 时 ,游戏 者 想象 右手 运动 来 使 平台 逆 时 针 运 动 。 这 样 ,小 球 就 可 以 相对 平稳 地 控制 
在 平台 上 。 通 过 游戏 ,游戏 者 在 体验 游戏 乐趣 的 同时 ,可 以 放松 身心 .可 以 有 目的 地 提高 专注 
度 使 控制 效果 更 好 , 拿 到 更 高 的 分 数 。 

为 了 更 好 地 了 解 实验 的 效果 和 体验 ,对 5 名 男生 进行 实验 ,观察 他 们 的 脑 电波 在 整个 实验 
过 程 中 的 变化 。 


图 10. 20 在 线 游 戏 控制 


3. 生物 反馈 效果 实验 

脑 电 作为 一 种 生理 信号 ,用 来 对 人 的 中 枢 神 经 系统 进行 功能 评估 ,同时 也 是 认识 功能 研究 
的 重要 途径 。 脑 电 图 在 人 处 于 不 同 的 意识 活动 下 会 在 某 些 频 段 表 现 出 振幅 变化 和 功率 变化 。 
如 1.1 节 所 说 , 脑 电 主要 分 为 delta(1 一 3Hz) ,theta(4 一 7Hz) alpha(8 一 12Hz) 以 及 beta(13 一 
30Hz) 四 种 波段 。 通 常 ,研究 者 通常 将 alpha 波 和 beta 波 作 为 人 精神 和 情绪 表现 的 指标 ,并 且 
alpha 通常 代表 放松 状态 ,beta 通常 代表 精神 振作 或 紧张 的 状态 。 另 外 ,SMR(13 一 15Hz) 波 和 
TBR(theta/beta ratio) 也 被 广泛 研究 ,并 且 越 来 越 多 的 证 据 表 明 , 人 的 注意 力 与 这 两 个 参数 有 
着 很 强 的 相关 关系 。 通 常 ,SMR 波 的 功率 与 人 的 注意 力 指 标 存在 正 相 关 关 系 , 而 TBR 跟 注 意 
力 指标 存在 负 相 关 关 系 。 

为 了 研究 游戏 实验 中 使 用 者 的 精神 状态 变化 情况 ,采集 并 分 析 了 上 述 5 位 受 试 者 在 游戏 
过 程 中 的 theta 波 、alpha 波 、.SMR 波 以 及 beta 波 的 功率 谱 。 图 10. 21 和 图 10. 22 显示 了 5 位 
受 试 者 在 实验 过 程 中 的 脑 电 波状 态 , 包 括 实验 前 期 .中 期 和 后 期 。 从 图 10. 21 发 现 ,5 位 受 试 - 
者 在 游戏 过 程 中 的 SMR 波 有 着 明显 提高 的 趋势 ,特别 是 实验 中 cz 和 dy。 这 说 明 受 试 者 在 游 
戏 过 程 中 的 注意 力 状 态 有 提高 的 趋势 ,一 方面 是 因为 游戏 者 逐渐 进入 游戏 状态 , 另 一 方面 也 体 
现 了 游戏 者 在 试图 调节 自己 的 注意 力 到 游戏 的 控制 上 。 然 而 ,也 看 到 yh、cz 和 dy 三 位 受 试 者 
的 SMR 波 并 非 一 直 保持 升 高 的 趋势 ,而 是 在 游戏 后 期 有 一 个 微微 下 降 的 趋势 ,这 可 能 跟 长 期 
集中 注意 力 导 致 精神 疲劳 有 关 。 但 是 这 个 下 降 趋势 是 微小 的 , 相 比 开始 时 的 SMR 波 水 平 仍 
有 很 大 的 提高 。 同 样 地 ,通过 图 10. 22 可 以 看 出 ,有 4 位 受 试 者 的 TBR 水 平 有 着 明显 降低 的 
趋势 ,特别 是 yh。 由 于 TBR 水 平 通常 与 注意 力 呈 负 相 关 关 系 , 所 以 TBR 分 析 情 况 与 SMR 相 
似 , 在 游戏 过 程 中 , 受 试 者 的 注意 力 水 平 有 上 升 趋势 。 

alpha 波段 通常 代表 人 的 放松 状态 ,并 且 较 高 的 alpha 波 表示 正在 抑制 无 关 的 内 部 及 心理 
活动 。 所 以 较 高 功率 的 alpha 波 可 以 反映 受 试 者 的 专注 度 和 放松 水 平 。 如 图 10. 23 所 示 , 除 


SMR 波 功率 /nuV? 


theta/beta 比 率 /TBR 


alpha 波 功率 /hV2 
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10.21 五 位 受 试 者 的 SMR 波 功 率 情况 
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图 10.22 五 位 受 试 者 的 TBR 情况 
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图 10.23 五 位 受 试 者 alpha 波 功率 情况 
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了 zr 以 外 ,其 他 受 试 者 在 游戏 过 程 中 的 alpha 波 功率 都 有 显著 的 提高 ,这 意味 着 受 试 者 的 状态 
通过 游戏 变 得 越 来 越 放松 ,并 且 逐 渐 沉 浸 到 游戏 当中 。 不 难看 出 ,设计 的 基于 脑 电 生物 反馈 游 
戏 在 提高 注意 力 水 平和 放松 身心 方面 具有 一 定 的 帮助 。 


10.3 应 用 于 人 机 情景 交互 的 视觉 图 像 处 理 技术 研究 


10.3.1 图 像 处 理 算法 简介 

本 节 将 介绍 包括 滤波 器 分割 算法 和 压缩 算法 在 内 的 算法 理论 ,滤波 器 包括 带 通 滤波 器 和 
统计 学 滤波 器 ,将 RANSAC 算法 视 为 分 割 算法 。3D 图 像 传输 压缩 技术 的 思想 是 在 两 个 I 帧 
信息 之 间 发 送 三 帧 P 帧 信息 。 使 用 点 云 库 (Point Cloud Library,PCL) 来 实现 这 些 算法 。 

1. 带 通 滤波 器 

3D 图 像 不 同 于 2D 图 像 ,每 个 像素 都 有 一 个 深度 值 , 这 个 深度 值 直接 与 对 象 到 传感器 的 
距离 有 关 , 越 靠近 摄像 的 对 象 , 其 深度 越 小 。 在 图 像 采集 过 程 中 ,可 以 根据 深度 值 去 除 不 感 兴 
趣 的 物体 。 该 步骤 计算 量 非 常 少 ,只 需 遍 历 每 一 个 点 即 可 。 

带 通 滤波 器 的 原理 非常 简单 ,该 滤波 器 的 关键 原则 是 允许 特定 的 波段 ,同时 屏蔽 其 他 波段 ， 
将 使 用 类 似 的 原理 ,根据 一 个 特定 领域 的 约束 来 消除 3D 图 像 的 背景 。 特 定 字段 可 以 是 X 轴 、Y 
轴 或 Z 轴 。 在 PCL 中 ,过 滤器 称 为 Pass Through 过 ”图 像 输 出 比例 
滤器 。 

可 以 选择 X 轴 、Y 轴 或 Z 轴 作 为 特定 字段 ,如 
图 10. 24 所 示 。 当 特定 字段 的 值 在 a 和 a, 之 间 时 ， 
此 范围 内 的 点 云 数据 将 全 部 保留 。 当 特定 字段 的 值 


100% 上 一 一 


小 于 ai 或 大 于 a, 时 ,该 范围 内 的 点 云 数据 将 全 部 删 2 al % ”特殊 字段 
除 。 可 以 使 用 以 下 公式 表示 带 通 滤波 器 ,其 中 = 和 Q 图 10 24 带 通 志波 器 原理 图 
分 别 表示 点 云 的 特定 字段 和 输出 比例 值 。 
0 tT < 0 
可 a 委 z 委 as (10. 34) 
0 rz 之 as 


2. 统计 学 滤波 器 

在 图 像 采 集 过 程 中 ,难免 存在 一 些 异 常 值 ,通常 这 些 异 常 有 一 个 共同 特点 一 一 在 异常 值 周 
围 存在 的 点 特别 少 ,这 导致 奇异 值 周 围 的 点 密度 普遍 偏 低 。 统 计 学 滤波 器 就 是 利用 这 个 特点 ， 
它 遍历 每 一 个 点 ,计算 每 一 个 点 与 其 邻近 的 点 的 平均 距离 ,点 的 平均 距离 比较 小 ,表示 该 点 附 - 
近 的 点 遍布 比较 密集 ,应 该 为 非 奇异 点 ; 点 的 平均 距离 相对 大 ,表示 该 点 附近 的 点 遍布 比较 稀 
下 ,应 该 为 奇异 点 。 

统计 学 滤波 器 的 主要 功能 是 根据 相 邻 点 之 间 的 距离 去 除 稀 朴 异常 值 ,统计 学 滤波 器 具有 
识别 稀疏 异常 值 的 功能 ,如 果 点 与 其 邻近 的 点 之 间 的 平均 距离 大 于 距离 阔 值 ,该 点 将 被 认为 是 
异常 值 。 首 先 计算 各 个 点 与 其 附近 点 之 间 的 平均 距离 ,然后 利用 平均 距离 向 量 的 标准 偏差 和 
均值 计算 距离 阔 值 ,最 后 利用 该 赋值 寻找 出 奇异 点 的 数据 点 。 

每 个 点 与 其 近邻 点 之 间 的 平均 距离 为 ,Ta ,3，…T，， 计 算 公式 为 


n= )/， G3) (10. 35) 


其 中 ,0) 是 第 i 个 点 与 其 第 j 个 邻近 点 的 距离 ， 计算 公式 为 : 
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0; = hy (point[i]. x — point[j J. xz) + (point[i]. y — point[j J]. y)” + (point[i]. z — point[j J]. z)” 
HO -36 
所 有 点 与 其 近邻 点 的 平均 距离 向 量 的 平均 值 为 


Ties = (Or) (10. 37) 
i=1 
所 有 点 与 其 近邻 点 的 平均 距离 向 量 的 方差 为 : 


e 一 二 >， 一 了 D) (10. 38) 
1 i=1 
由 式 (10. 35) 一 式 (10. 38) 可 计算 距离 阔 值 为 : 
De =T an 4 二 记 二 上 39) 
如 果 满 足 式 (10. 40) , 则 认为 该 点 为 奇异 点 ,有 
L; 2 Onkol (10. 40) 


奇异 值 点 分 布 如 图 10. 25 所 示 。 其 中 ,图 10. 25(a) 中 的 奇异 值 点 分 布 在 各 圆圈 处 ,显然 ， 
奇异 值 周围 分 布 密度 比较 低 ; 图 10. 25(b) 为 处 理 过 的 图 像 , 原 图 中 的 奇异 值 全 部 被 清除 ,统计 
学 滤波 器 的 效果 非常 明显 。 


油 a 

训 

2 时 

3 

” Ss 
EE 


(a) 原 图 (b) 经 过 统计 学 滤波 器 处 理 过 的 图 像 
图 10.25 统计 学 滤波 器 处 理 效果 图 


3. 分 割 算 法 

RANSAC 算法 将 从 原始 输入 数据 集中 选择 多 组 假设 的 非 异 常 值 ,然后 ,尝试 利用 假设 的 
非 异 常 值 进行 构建 模型 ,并 利用 损失 函数 来 测试 其 他 点 ,因此 ,能 够 获得 对 应 于 该 模型 的 非 异 
常 值 。 如 果 某 个 模型 对 应 的 非 异常 值 点 的 数目 达到 最 大 ,就 可 以 认为 该 模型 为 最 优 模 型 ,最 后 
得 到 全 部 非 异 常 值 。 在 实际 应 用 中 ,模型 的 类 型 是 已 知 的 ,假设 的 非 异 常 值 用 来 获取 模型 的 参 
数 。 如 果 非 异常 值 集 包含 最 大 数量 的 非 异 常 值 ,可 以 认为 估计 的 模型 是 相当 好 的 。 最 后 ,该 算 
法 将 删除 异常 值 ,并 保留 非 异 常 值 ,之 后 ,最 小 二 乘法 将 用 于 通过 使 用 内 部 的 所 有 成 员 来 重新 
估计 模型 。 : 

自 1981 年 以 来 ,在 计算 机 视觉 和 图 像 处 理 领 域 ,很 多 研究 者 将 RANSAC 算法 作为 基础 
工具 。Torr 等 人 指出 RANSAC 算法 能 够 正确 选择 冰 值 ,确定 哪些 数据 点 是 满足 特定 参数 集 
确定 的 模型 。 如 果 阅 值 选 择 太 大 ,那么 很 多 异常 值 不 能 去 除 ,也 就 是 说 ,利用 多 组 非 异 常 值 建 
立 的 模型 都 是 等 价 的 ,如 果 阅 值 选 择 太 小 ,参数 估计 可 能 会 有 波动 ,仅仅 是 简单 地 添加 或 者 去 
除 一 组 数据 集 。 为 补偿 这 个 不 足 之 处 ,Torr 等 人 提出 了 利用 M- 估 计 器 和 最 大 似 然 估 计 理 论 
修改 的 思想 。Tordoff 提出 将 输入 数据 集 相 关联 的 先前 概率 的 MLESAC 考虑 到 RANSAC 算 
法 中 ; 如 果 输 入 数据 的 先 验 信息 是 已 知 的 ,并 考虑 了 选择 的 非 异 常 值 集中 有 可 能 存在 异常 值 。 
Chum 提出 了 指导 抽样 算法 。 为 了 减少 计算 量 ,Chum 等 还 提出 了 RANSAC 的 随机 化 版 本 ， 
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先 随机 选择 包含 比较 少 的 点 数据 集 来 评估 假设 的 模型 的 性 能 ,可 以 剔除 一 些 性 能 比较 差 的 假 
设 模型 ,从 而 减少 计算 量 。 

其 中 ,和 迭代 次 数 应 该 被 确定 , 即 应 该 选择 多 少 组 假设 的 非 异 常 值 来 评估 模型 ,9 表示 
RANSAC 算 法 从 输入 数据 集中 选择 非 异常 值 集 的 概率 。k 表示 至 少 可 以 用 于 估计 模型 参数 
的 点 数 。e 表示 每 次 选择 单个 点 时 选择 一 个 非 异 常 值 的 概率 ,e 等 于 非 异 常 值 点 数 除 以 总 的 点 
数 , 通 常 选择 s 小 一 点 比较 好 。6 代表 置信 度 ,一般 选择 6 为 0.995。 求 解 公式 如 下 : 

二 (10. 41) 
log(l 一 e ) 

4. 压缩 算法 

目前 的 视频 压缩 准则 是 H. 264, 该 准则 是 一 种 高 效 的 视频 压缩 技术 ,在 不 影响 视频 质量 
的 情况 下 ,该 编码 器 可 以 将 视频 文件 的 大 小 减少 至 20% 以 下 ,显然 ,H. 264 编码 技术 降低 了 传 
输 视 频 对 宽带 的 要 求 , 同 时 也 降低 了 对 存储 空间 的 需求 。 视 频 压 缩 的 工作 核心 是 去 除 每 帧 中 
宛 余 的 内 容 ,实现 快速 传输 和 保存 视频 内 容 的 目的 。 在 发 送 和 存储 文件 之 前 ,视频 文件 必须 使 
用 一 种 压缩 算法 对 视频 进行 压缩 和 编码 。 在 视频 接收 端 和 播放 端 , 必 须 有 对 应 的 解压 视频 文 
件 算法 ,然后 可 将 被 压缩 的 视频 文件 还 原 。 解 码 后 的 视频 数据 和 解码 前 的 视频 数据 应 当 完全 
一 致 。 
由 于 编码 器 的 设计 者 不 同 ,所 采用 的 编码 策略 也 有 很 大 差异 ,导致 不 同 标准 的 视频 编码 需 
之 间 是 不 兼容 的 ,所 以 压缩 标准 和 解压 标准 必须 保持 一 致 ,一 种 压缩 算法 输出 不 能 作为 男 一 种 
解压 算法 的 输入 ,否则 将 出 现 乱码 。 

对 于 压缩 算法 性 能 的 评估 ,基本 上 是 基于 压缩 比特 率 、 质 量 和 延 时 ,用 户 会 根据 自身 项 目 
的 需求 选择 不 同 的 压缩 标准 。 对 于 不 同 的 压缩 标准 ,压缩 比特 率 、 质 量 和 延 时 方面 的 表现 是 不 
相同 的 。 即 使 对 于 同一 个 标准 ,压缩 比特 率 、 质 量 和 延 时 方面 也 有 可 能 不 完全 相同 。 所 以 ,在 
项 目 中 ,使 用 指定 的 编码 标准 无 法 保证 压缩 质量 一 致 。 

压缩 算法 的 主要 思想 是 减少 数据 传输 量 ,其 关键 思想 是 将 P 帧 结合 到 工 帧 中 进行 3D 图 像 
传输 。 具 有 相对 较 大 尺 十 的 工 帧 包括 所 有 视觉 信息 ,而 具有 相对 
较 小 尺寸 的 P 帧 仅 包 括 与 前 一 帧 相 比 后 的 当前 帧 的 较 小 变化 的 
信息 。 本 系统 要 求 操 作 人 员 的 脸 部 运动 幅度 不 能 过 大 。 因 此 ， 
面部 信息 将 被 视 为 P 帧 的 信息 。 为 了 获取 P 帧 的 信息 ,将 使 用 
Haar-Face 检测 算法 和 鼻尖 检测 算法 来 获取 操作 者 脸 部 信息 。 
压缩 算法 的 想法 是 在 两 个 I 帧 信息 之 间 发 送 多 个 P 帧 信息 ,如 
图 10. 26 所 示 。 : 

在 接收 端 获 得 P 帧 信息 后 ,为 了 能 够 得 到 了 帧 信息 ,必须 使 用 运动 矢量 进行 评估 。 在 图 像 
获取 端 , 利 用 人 脸 检测 技术 检测 出 鼻尖 的 位 置 , 利 用 工 帧 的 鼻尖 位 置信 息 和 了 帧 鼻尖 位 置信 息 
的 差 值 可 以 得 到 运动 向 量 。 这 样 的 处 理 方法 用 在 考虑 两 帧 变化 的 信息 仅仅 是 脸 部 信息 ,如 果 
考虑 了 其 他 部 位 信息 的 变化 ,必须 使 用 全 部 信息 进行 矢量 评估 。 鼻 尖 的 运动 向 量 为 

GZ yz) 一 7iTCZy yzZ) — npr,y ,2) (10. 42) 
其 中 ,ni1(z,y,z) 为 1 帧 的 鼻尖 位 置信 息 ,np (xz,y,z) 为 P 帧 的 鼻尖 位 置信 息 。 

参考 帧 为 1 帧 信息 ,目标 帧 为 P 帧 信息 (如 图 10. 26 所 示 ) ,利用 参考 帧 和 目标 帧 进行 对 
比 ,可 以 得 到 差 值 ,最 终 得 到 运动 向 量 。 如 图 10. 27 所 示 为 运动 向 量 评估 解析 图 。 得 到 运动 向 
量 后 ,可 以 根据 运动 向 量 、I 帧 和 PP 帧 信息 可 以 得 到 最 新 的 完整 的 新 一 帧 信息 。 

第 一 步 , 先 利用 运动 向 量 和 了 帧 信息 得 到 准确 的 脸 部 信息 ,ni(z,y,z) 为 最 终 自 人 尖 位 置 


.图 10. 26 :压缩 算法 原理 图 
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ni(zys2) =np(rs yz) yr, yz) (10. 43) 
第 二 步 , 将 得 到 的 脸 部 信息 和 工 帧 信息 进行 融合 ,得 到 最 终 的 新 一 帧 信息 。 


离散 Cosinus 变 换 
量化 


差异 一 


RLE 
本 帮 霍 夫 曼 编码 


图 10.27 运动 向 量 评估 解析 图 


以 下 使 用 Haar-Face 检测 算法 检测 人 脸 。 该 算法 中 ,Haar-Like 特征 的 值 将 用 于 训练 弱 
分 类 器 ,将 弱 分 类 器 用 于 构成 强 分 类 器 。 最 后 , 强 分 类 器 组 成 级 联 ,每 个 阶段 的 强 分 类 器 由 不 
同 个 数 的 弱 分 类 器 集成 ,阶段 在 前 面 的 强 分 类 器 所 包括 的 弱 分 类 器 的 数量 比较 少 。 

1) Haar-Like 特征 

为 了 减少 与 原始 输入 数据 相 比 的 类 别 差 异 , Haar-Like 特征 将 被 视 为 机 器 学 习 算 法 的 输 
入 数据 ,这 使 得 分 类 咒 实 现 更 加 容易 。 

14 个 特征 原型 被 分 为 三 类 ,包括 边缘 特征 (Edge features) 线性 特征 (Line features) 和 中 
心 -边缘 特征 (Center-surround features) ,如 图 10. 28 所 示 。 特 征 值 的 计算 过 程 如 下 。 假 设 图 
像 的 基本 单元 是 像素 为 WX 的 窗口 ,第 i 个 矩形 特征 的 像素 累计 值 为 p; 二 (zx,y,w,h,0)， 
其 中 i 二 1,2,3,…,N ,NN 是 窗口 中 的 特征 数目 ,0 寺 zx ,x 十 w 宇 W,0y,y 二 h 宇 H,0€1{0°， 
45"),{z,y) 是 和 矩形 特征 右 下 角 像 素 点 的 坐标 ,w 和 有 分 别 是 矩形 特征 的 宽 和 高 ,9 是 旋转 和 矩形 
特征 的 旋 动 角度 。 特 征 的 值 为 


N 
pe (10. 44) 


边 终 特征 ， Img | 国 DS > 


线 征 ， | 年 | | 国 | 图 加 令 心 ® ~ 
ca 器 > 


图 10. 28 ”Haar-Like 特征 种 类 


其 中 ,w; 是 第 i 个 和 矩形 特征 的 权重 。 
每 个 窗口 包含 矩形 特征 的 个 数 为 : 
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| | 

N=XY. (W+1—w~3—)* (H+1—h 3—) (10. 45) 

昌 过 十 vat 
N=—XY* (W+1—a~ 2): (H+1—a 7 ) (10. 46) 

i 和 
> h 

为 提高 计算 特征 值 的 速度 ,可 以 使 用 积分 图 像 来 计算 特征 值 ,计算 原理 如 下 : 

和 (zy) 一 >) Dplr' sy) (10. 47) 
z' 和 zy <y 

r(Zyy) 一 r(Zzy 一 1) 十 如 zyy) (10. 48) 


其 中 ,ii(z,y) 是 整体 图 像 的 像素 和 ,p(xr,y) 是 点 (x ,y) 的 像素 ,r(xz,y) 是 图 像 第 行 累积 的 
像素 和 ,如 图 10. 29 所 示 。 


10.29 积分 图 像 原 理解 释 图 


积分 图 像 计算 示例 如 图 10. 30 所 示 , 由 图 可 知 , 和 矩形 1 中 的 积分 图 像 的 特征 值 于 加 和 为 
ii(zs。，ya) ,矩形 2 中 的 积分 图 像 的 特征 值 赤 加 和 为 ii(zs ,ys) 一 ii(x。，y。)，, 和 矩形 3 中 的 积分 
图 像 的 特征 值 丢 加 和 为 荆 (z,y.) 一 研 (zu,y。), 和 矩形 4 中 的 积分 图 像 的 特征 值 玛 加 和 为 
人 全 的 人 ar 信人 本 ed rr AG 和 人 


10. 30 ”积分 图 像 计算 示例 
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2) Haar-Like 特征 分 类 器 
Haar-Like 分 类 器 的 目的 是 检测 输入 数据 是 否 为 人 脸 的 信息 。 为 了 检测 人 某 部 位 的 特 
征 ,许多 分 类 器 设计 为 检测 人 的 特征 。 人 体 某 些 相关 关节 将 使 用 支持 向 量 机 、K-means 和 隐 
马尔 可 夫 模 型 对 其 进行 估计 。 这 里 先 介绍 弱 分 类 器 和 强 分 类 器 的 训练 过 程 。 
针对 各 个 样本 集 ,计算 所 有 输入 图 像 的 特征 值 , 利 用 排列 技术 将 特征 向 量 按照 从 小 到 大 进 
行 排列 ,然后 为 该 特征 向 量 选 择 一 个 最 好 阔 值 ,最 终 形成 一 个 弱 分 类 器 。 弱 分 类 器 可 以 由 以 下 
公式 表示 : 
| be b> 
w(p;) 一 CLO S03 
0 ”其 他 


- 其 中 ,y; 是 阔 值 ,z; 用 于 修正 不 等 式 两 边 符号 的 方向 ,p; 是 窗口 特征 的 值 。 强 分 类 器 可 以 用 
以 下 公式 表示 ,其 中 pg; 是 弱 分 类 器 的 权重 ,J 是 弱 分 类 器 的 数量 。 


J. 

1 
1 a, 
这 pit p; 2 2 


(10. 51) 
lo 其 他 

3) 级 联 器 

级 联 器 由 一 些 强 分 类 器 组 成 ,用 于 消除 每 个 阶段 不 符合 条 件 的 候选 数据 ,如 图 10. 31 所 


示 。 随 着 候选 数据 通过 的 阶段 越 来 越 多 ,候选 人 通过 强 分 类 器 的 机 会 越 来 越 小 。 如 果 候 选 数 


据 通过 所 有 阶段 , 则 该 候选 数据 为 面部 信息 。 
DT 


图 10.31 级 联 器 原理 图 


以 下 介绍 鼻尖 检测 算法 。 为 了 获得 更 准确 的 面部 区 域 , 应 该 检测 鼻尖 。 许 多 研究 人 员 更 
加 重视 通过 一 些 策 略 来 检测 人 体 特 殊 部 位 的 位 置 。 在 检测 鼻尖 位 置 方面 ,很 多 文献 都 介绍 了 
方法 ,其 中 比较 经 典 的 实现 方法 为 通过 计算 曲面 的 曲率 来 寻找 鼻尖 位 置 。 根 据 曲 率 获 得 鼻尖 
位 置 的 核心 概念 是 利用 人 脸 的 曲率 分 布 特点 寻找 鼻尖 信息 ,其 中 包括 平均 高 斯 曲率 和 主 曲率 。 
在 利用 曲率 方面 ,利用 Shape Index 特征 确定 鼻尖 的 做 法 比较 流行 ,因为 鼻尖 的 Shape Index 
特征 在 脸 部 呈现 一 定 的 聚集 性 ,很 容易 区 别 出 鼻 尖 的 位 置 ,然后 利用 阔 值 找 出 符合 鼻尖 Shape 
Index 特征 的 区 域 ,该 区 域 的 中 心 即 为 鼻尖 位 置 。 但 是 该 方法 计算 量 大 ,不 适合 实时 检测 
鼻尖 。 

本 设计 将 执行 由 粗 到 精 的 方法 来 检测 鼻尖 的 位 置 ,过 程 如 下 。 

首先 ,3D 人 脸 将 根据 y 轴 的 值 分 为 多 个 切片 ,然后 选择 相对 较 大 的 遍历 步伐 4, 来 检测 
鼻尖 所 在 的 粗糙 区 域 , 以 提高 搜索 速度 。 最 后 , 当 鼻 尖 的 位 置 粗 略 定位 后 ,将 在 该 邻近 区 域 中 
重复 搜索 鼻尖 ,并 以 较 小 的 遍历 步 长 4, 重复 。 如 图 10. 32 所 示 ,在 每 个 切片 中 ,对 每 个 点 绘 
制 一 个 圆 , 圆 与 曲面 相交 于 两 点 ,三 点 将 组 成 一 个 三 角形 ,三 角形 的 高 度 将 被 求解 出 来 。 在 切 
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片 中 ,三 角形 的 高 值 较 大 的 点 将 被 视 为 鼻尖 的 备 选 点 ,利用 该 过 程 处 理 所 有 切片 以 确定 一 些 备 
选 点 。 最 后 ,将 在 备 选 点 群 中 找到 正确 的 鼻尖 。 


图 10. 32 鼻尖 检测 原理 图 


10.3.2 基于 人 机 情景 交互 的 图 像 处 理 算 法 设计 

1. 系统 介绍 

本 系统 整体 流程 包括 14 个 模块 (如 图 10. 33 所 示 ) ,主要 由 两 大 部 分 组 成 ,分 别 是 服务 操 
作 端 和 客户 交互 端 。 服 务 操作 端 利用 深度 摄像 头 采集 操作 者 上 半身 的 信息 ,经 过 图 像 处 理 后 ， 
将 操作 者 的 命令 组 合成 数据 包 , 一 起 发 送 到 客户 交互 端 。 客 户 交互 端 接收 到 网 络 传 过 来 的 数 
据 , 然 后 将 数据 包 分 包 , 存 人 不 同 的 缓冲 区 ,显示 相关 信息 ,这 需要 一 台 投影 仪 和 贴 着 玻璃 幕布 
的 玻璃 板 , 最 终 可 以 在 客户 面前 显示 相关 3D 信息 。 在 客户 交互 端 ,客户 可 以 通过 leap motion 
等 交互 设备 和 3D 投影 图 像 交 互 ,交互 结果 同样 会 显示 在 客户 交互 端的 玻璃 板 上 ,该 结果 同时 
通过 局 域 网 传 到 服务 操作 端 。 


图 10.33 系统 结构 描述 图 


第 10 章 人 机 交互 | 263 


se 


服务 操作 端 至 少 包括 操作 者 0、 深 度 摄像 机 1、 图 像 处 理 模块 2、3D 图 像 显 示 模 块 3、 服 务 
操作 端 命令 处 理 模 块 4、 数 据 发 送 \ 接 收 模 块 5。 其 中 ,深度 摄像 机 1 在 本 次 设计 中 使 用 Kinect 
传感器 ,目的 是 获取 操作 者 的 3D 点 云 数据 ; 图 像 处 理 模块 2 用 于 处 理 3D 点 云 数据 ,对 3D 点 
云 数 据 进行 滤波 、 分 割 ; 3D 图 像 显 示 模 块 3 用 于 显示 采集 处 理 后 的 信息 和 交流 结果 信息 ; 服 
务 操作 端 命 令 处 理 模块 4 用 于 对 操作 者 的 命令 处 理 和 交互 结果 的 解析 ,为 显示 \ 发 送 做 好 准 
备 ; 数据 发 送 \ 接 收 模块 5 用 于 发 送 3D 点 云 信 息 和 操作 者 的 命令 、 接 收 客户 交互 端 交 互 的 
结果 。 

在 客户 交互 端 包括 数据 发 送 \ 接 收 模块 6、 客 户 交 互 端 命令 处 理 模块 7、 图 像 处 理 模 块 8、 
泻 染 模块 9、 投 影 仪 10、3D 图 像 显示 模块 11、 手 的 信息 处 理 模块 12、 客 户 的 操作 手 13。 其 中 ， 
_ 客户 交互 端 是 移动 机 器 人 ,包括 以 上 模块 。 数 据 发 送 \ 接 收 模块 6 用 于 接收 服务 操作 端 传 输 过 
来 的 点 云 信息 和 指令 .传输 客户 交互 端 交 流 的 结果 ; 客户 交互 端 命令 处 理 模块 7 是 对 操作 者 
的 指令 和 交流 结果 的 解释 ,为 显示 \ 发 送 命令 做 好 准备 ; 图 像 处 理 模块 8 是 处 理 点 云 信息 和 命 
令 ,把 点 云 数 据 和 命令 存在 不 同 的 缓存 区 ,并 把 点 云 数 据 赋 给 PCL 对 象 ,为 演 染 做 好 准备 ; 泻 
染 模块 9 把 点 云 信息 和 命令 在 客户 交互 端的 电脑 GPU 上 泻 染 ; 投影 仪 10 把 泻 染 好 的 信息 投 
影 到 贴 有 幕布 的 玻璃 板 上 ; 手 的 信息 处 理 模块 12 处 理 leap motion 等 交互 设备 采集 到 一 些 信 
息 , 如 手掌 法 线 、 手 指向 量 、 手 掌 位 置 客户 的 语音 等 ,在 利用 这 些 信息 之 前 也 要 进行 滤波 处 理 。 
交互 结果 会 更 新 到 幕布 玻璃 板 上 ,同时 也 反馈 到 服务 操作 端 ,服务 操作 端 接收 到 命令 后 ,做 出 
相应 的 反应 ,然后 传送 到 客户 交互 端 更 新 3D 显示 内 容 , 从 而 完成 一 次 交互 动作 。 

移动 机 器 人 具有 自主 避 障 .自主 平衡 ,人 脸 识 别 . 跟 随 等 功能 。 利 用 自主 避 障 可 以 绕 过 障 
碍 物 ,重新 规划 路 线 ; 自主 平衡 保证 机 器 人 自身 平衡 ; 人 脸 识 别 是 为 了 更 好 地 为 顾客 服务 , 记 
录 顾 客人 脸 信 息 ,使 得 交互 更 加 友好 ; 跟随 功能 可 以 实现 跟随 顾客 移动 。 

2. 系统 整体 算法 设计 介绍 

在 服务 端 采集 到 整个 场景 的 3D 图 像 ( 如 图 10. 34 所 示 ) ,然后 利用 带 通 滤波 器 把 背景 去 
除 。 为 了 使 采集 到 的 3D 图 像 的 点 密度 更 加 平滑 ,使 用 统计 学 滤波 器 去 除 奇异 点 。 接 着 ,使 用 
RANSAC 分 割 算法 将 人 体 上 半身 的 3D 图 像 提 取出 来 。 为 了 减少 传输 量 ,利用 I.P 帧 技术 与 
人 脸 检 测 技术 相 结合 ,进行 编码 图 像 信息 ,最 后 使 用 TCP 协议 将 压缩 后 的 图 像 传输 到 客户 端 ， 
在 客户 端 进行 解码 和 解压 ,最 终 将 图 像 泻 染 出 来 。 


图 像 解 码 与 泻 染 


客户 端 


RANSAC 
分 割 算 法 


服务 端 采 集 图 像 


图 10. 34 系统 框图 


3. 带 通 滤波 器 设计 

在 带 通 滤波 需 中 ,将 Z 轴 作 为 特定 字段 ,其 中 ai 二 0.0 和 ws 三 1.0。 因 此 ,如 果 使 用 带 通 
滤波 需 来 处 理 点 云 数据 , 则 仅 保留 深度 为 0 一 1m 的 点 。 带 通 滤波 器 比较 简单 ,只 需 遍 历 所 有 
点 , 设 定 Z 轴 上 的 阔 值 即 可 ,这 样 就 很 容易 去 除 背 景 。 
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4. 统计 学 滤波 器 设计 

根据 统计 学 滤波 器 的 理论 知识 可 知 实现 该 滤波 器 的 算法 步骤 如 下 。 

步骤 1: 计算 全 部 点 相对 于 这 些 点 附近 y 个 相 邻 点 的 平均 距离 ,假设 全 部 点 的 平均 距离 为 
Ti ,Ta ,TT ，… ,了 ,n 是 点 云 信息 的 点 数 ,0; 代表 点 和 最 邻近 的 点 集中 第 7 点 之 间 的 距离 。 

步骤 2: 根据 式 (10. 37) 和 式 (10. 38) 计 算 平均 距离 向 量 (Ti ,TT ,Ts,…,T 卫 , ) 的 平均 值 和 方 
差 。T,os 表示 平均 距离 向 量 的 平均 值 ,c 表示 平均 距离 向 量 的 方差 。 

步骤 3: 根据 式 (10. 39) 计 算 距 离 浆 值 ,usu 表示 距离 准 值 ,std ,i 表示 标准 偏差 倍数 。 

步骤 4: 根据 距离 冰 值 ussoid ,删除 异常 值 , 具 有 过 高 平均 距离 的 点 将 被 视 为 异常 值 , 并 
将 被 删除 , 非 异 常 值 被 保留 以 构建 新 的 点 云 数据 集 并 且 数 据 集 可 以 逐 点 泻 染 。 

经 过 多 次 尝试 ,统计 学 滤波 器 参数 设置 为 : Y= 二 20 和 std wu 一 1.0。 

统计 学 滤波 器 的 伪 代 码 如 下 : 


1. y< 最 近 近 邻 点 数 \\ 
B20 
3 TON 
4. 了 n 一 点 云 数 据 的 大 小 \\ 
5. while i <n\\ 
1) do while j <7Y\\ 
(1) do 0,.<—(point[il].x- point[j].x)’\\ 
(2) < Cpointld | Wr podnt [3d WN 
(3) 0,<—(point[i].z—- point[j].2z)’\\ 


(4) 9[jJ]<—VO. +6 +D \\ 
(5) T[iJ<-0[jJ/n +T[Li] \\ 
(6) jj+1\\ 
2) j<—0\\ 
3) 1a=-4.41\\ 
PaO NN 
. Whilé 1 <Aa\\ 
doT Ti/n+T ONN 
AN 
9. while i <n\\ 
1) do:yy<—(T[i] -Tn) /n+ yy\\ 
2) 141\\ 
LO e<— Vyy NN 
AN 
12. i<0; \\ 
13; j<0;,\ 
14. while i <n\\ 
TL RS 
then [\\ 
index[ j |]<—i\\ 
I 
,这 二 守 下 全 ] NAN 
else [i<1+i]\\ 


-a 
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5. 分 割 算法 设计 

分 割 算法 步骤 如 下 。 

步骤 1: 针对 具体 问题 ,设计 一 个 具体 模型 来 选择 非 异 常 值 集 。 一 个 具体 的 模型 可 以 是 平 
面 模型 . 线 模型 和 圆 模型 等 。 

步骤 2: 计算 迭代 次 数 n。6 表示 RANSAC 算法 从 输入 数据 集中 选择 非 异 常 值 集 的 概 
率 ; 表示 至 少 可 以 用 于 估计 模型 参数 的 点 数 ; s 表示 每 次 选择 单个 点 时 选择 一 个 非 异 常 值 
的 概率 。 

步骤 3; 从 原始 输入 数据 集中 随机 选择 n 组 假设 非 异 常 值 集 ,每 组 包括 个 点 。 

步骤 4: 使 用 组 假设 非 异常 值 集 来 构建 模型 。 

步 又 5: 根据 阀 值 剔除 相应 模型 的 所 有 异常 值 , 得 到 非 异 常 值 集 。 将 由 用 户 设计 。 通 
过 最 小 二 乘法 重新 使 用 刚刚 得 到 的 非 异 常 值 集 来 构建 模型 。 

步骤 6: 记录 非 异常 值 集 的 点 数 大 小 ,使 用 非 异常 值 集 的 点 数 大 小 与 目前 最 大 非 异 常 值 集 
点 数 进行 比较 。 如 果 这 个 数字 大 于 目前 最 大 非 异常 值 集 点 数 , 该 模型 的 参数 就 会 得 到 保存 ,而 
这 个 模型 的 非 异常 值 集 点 数 将 成 为 目前 最 大 非 异常 值 集 点 数 。 

步骤 7: 重复 步骤 4 一 6, 直 到 重复 次 数 达 到 迭代 次 数 。 

在 RANSAC 算法 中 ,这 里 将 一 个 平面 模型 作为 特定 模型 ,并 将 靖 值 选择 为 0. 1。 

6. 人 脸 识别 算法 设计 

1) 强 分 类 器 设计 

这 部 分 将 利用 若干 个 样本 训练 出 弱 分 类 器 ,然后 利用 弱 分 类 器 和 对 应 的 权重 组 合成 强 分 
类 器 ,具体 步骤 如 下 : 


OO 输入 样本 (xivyi)…(xwryr)vyi 二 1Yi 二 表示 输入 样本 是 脸 。 


@ 初始 化 权重 7; = 二 


©®@ while j=1,. ,J: 
。 计算 每 个 图 像 的 特征 值 。 
。 设置 阔 值 ,根据 公式 获取 弱 分 类 器 的 结果 , 并 计算 分 类 误差 : 
= 77; CO. B52 
i: y) wp;) 
。 不 断 设 置 阔 值 , 求 出 最 优 阔 值 , 即 为 使 得 分 类 误差 最 小 的 阔 值 , 训练 出 一 个 弱 分 类 器 。 
。 计算 弱 分 类 器 对 应 的 权重 : 


Dr eXPK 有 7 《让 


tr Ca = (10.53) 
之 9 «exp(—B; * y; * w(p;)) 


jh -es(), 
@ 得 到 强 分 类 器 为 : 


| v(p;) sign( 7 4 up)】 (10.54) 
j=1 


2) 级 联 分 类 器 设计 

一 般 训 练 出 来 的 强 分 类 器 检测 率 不 是 很 高 ,为 了 增 大 检测 率 ,也 会 增 大 误 识 率 ,因为 增 大 
检测 率 需 要 减 小 阅 值 , 减 小 阅 值 则 导致 误 识 率 增 大 ,所 以 考虑 级 联 来 均衡 两 者 的 矛盾 关系 。 具 
体 级 联 分 类 训练 伪 代 码 如 下 : 
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中 设 定 每 个 强 分 类 器 要 达到 最 小 检测 率 d, 每 一 个 强 分 类 器 的 误 识 率 不 能 超过 f, 理想 目标 的 误 识 率 
为 Bt 
@P 为 人 脸 样本 ,N 为 非 人 脸 样 本 , Do。 =1.0,F。= 4.0。 
® i=0。 
©@ while F, > 
a 迁 乱 书 吉 
ea 7 
。 while F, >f*F,_i 
站 这 
。 利 用 adaBoost 算法 训练 强 分 类 器 ,其 中 包含 ni 个 弱 分 类 器 ; 
。 求 出 级 联 分 类 器 的 检测 率 D; 和 误 识 率 F;; 
。 While D, <D,.i *d 
。 减少 第 i 层 强 分 类 器 的 国 值 
。 求 出 级 联 分 类 器 的 检测 率 D; 和 误 识 率 F， 
将 误 识 到 的 图 像 加 入 Y 样本 集中 


7. 鼻尖 检测 算法 设计 

本 系统 设计 将 使 用 由 粗 到 精 的 方法 来 检测 鼻尖 的 位 置 ,过 程 如 下 。 

步骤 1: 3D 人 脸 将 根据 y 轴 的 值 分 为 多 个 切片 。 

步骤 2: 选择 相对 较 大 的 遍历 步伐 4。 来 检测 鼻尖 所 在 的 粗糙 区 域 ,在 每 个 切片 中 ,以 每 
个 点 为 圆心 绘制 一 个 圆 , 圆 与 曲面 相交 于 两 点 ,三 点 将 构成 一 个 三 角形 ,由 此 得 到 三 角形 的 高 ， 
对 比 当前 保存 的 高 的 最 大 值 ,如 果 该 点 构成 的 三 角形 的 高 比 当前 保存 的 高 的 最 大 值 还 要 大 , 则 
保存 该 点 的 位 置信 息 , 该 点 对 应 的 三 角形 的 高 的 值 为 当前 保存 高 的 最 大 值 。 

步骤 3: 以 较 大 的 遍历 步 长 4, 粗略 遍历 切片 后 ,可 确定 鼻尖 的 大 致 位 置 。 

步 又 4: 当 鼻尖 的 位 置 粗略 定位 后 ,将 在 该 邻近 区 域 中 重复 搜索 鼻尖 ,并 以 较 小 的 遍历 步 
长 4。 重复 ,一 般 d, 为 1。 

步骤 5: 同样 地 ,在 每 个 切片 中 ,以 每 个 点 为 圆心 绘制 一 个 圆 ,同步 又 2。 

步骤 6: 以 遍历 步 长 4, 为 1 遍历 切片 后 ,可 确定 最 终 的 鼻尖 位 置 。 

实现 程序 的 伪 代 码 如 下 。 


1. zx<- 圆 的 半径 
2. N< -切片 数 
Se 0 
轩 。 下 专人 


max 


index sx™0 
while i<N 
1) do n 一 第 i 个 片段 中 的 点 数 
2) while jn\\ 
(1) temp<—0 
(2) do while kn 
@ 9,.<(slices[i][j].x- slices[i][k]. x)’ 


@ 0,<—(slices[i][j].z- slices[i][k].z)’. 


a Un 
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ee 


@ if((slices[ i][j].y== slices[i][j]. y)g&(k! = J)&&(CO == r)) 
。 point[ temp |<—slices[ i |[k]| 
。 temp<-temp+1 ] 
© ifttemp ==2) 
。 h,.<(slices[i][j].x- (point[0].x + point[1]. x)/2)’ 
。 h,<(slices[i][j].z- (point[0].z + point[1].z)/2)’ 
,hh Th 
。 temp<—0 
。 if(h> ho) 
be 
“ Tader si 
” 。 break 
© k<—k+1 
C3 e008 
3) 1<=1i +20;j<=0 \\ 
7. Ne ee 十 10\\ 
8.. edjndex .= 10\\ 
9 bse0N\\ 
10. while i<N\\ 
1) do n 一 第 i 个 片段 中 的 点 数 
2) while jn\\ 
(1) temp<—0 
(2) do while kn 
@ 0,.<(slices[i][j].x- slices[i][k].x)? 
©@ 90,<—(slices[i][j].z- slices[i][k].z)’ 


@0<VO +0,. 
@ if((slices[ 1 |][j].y== slices[ #7].Yy)SEECk! = jEEC0 == 7r)) 
。 point[ temp |<—slices[ i |[k]| 
®temp=<—temp.+1 
© if(temp == 2) 
。 h,.<-(slices[i][j].x- (point[0].x+ point[1].x)/2)’ 
。 h,<(slices[i][j].z- (point[0].z+ point[1].2z)/2)’ 
i 
。 Tenp™*0 
“二 
。h < 
"De .1 
。 break 
© k<—k+1 
(3) J mb 0 
3) i<—i+1,j<0\\ 


10.3.3 ”图像 处 理 研 究 实验 过 程 及 其 结果 

1. 实验 准备 

通过 Kinect 传感器 可 以 获得 3D 点 云 数据 (这 里 使 用 Kinect V1 传感器 ) ,为 了 获得 3D 图 
像 的 深度 信息 ,具有 红外 滤波 器 的 传统 COMS 图 像 传感器 将 检测 红外 发 射 器 发 射 的 红外 点 
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阵 。 物 体 与 Kinect 传感器 之 间 的 距离 变化 将 导致 红外 点 阵 中 点 的 位 置 及 尺 二 的 变化 。 
Kinect 传感器 的 深度 分 辨 率 为 1280X1024, 传 感 器 能 够 检测 1 一 3. 5m 的 深度 距离 范围 。 传 感 
器 的 视觉 区 域 是 水 平方 向 为 57", 垂 直方 向 为 43" 的 矩形 圆锥 体 。 传 感 器 也 可 以 通过 RGB 相 
机 获得 RGB 图 像 。Kinect V1 传感器 的 结构 原理 图 如 图 10. 35 所 示 。 
RGB 摄像 头 _ 
红外 发 送 器 红外 接收 器 


3D 图 像 传感器 
图 10. 35 Kinect V1 传感器 的 结构 原理 图 


实验 中 使 用 Kinect 传感器 来 获取 3D 点 云 数据 ,通过 两 台 计 算 机 实现 图 像 处 理 算法 ,分别 
为 服务 端 和 客户 端 。 服务器 用 于 获取 和 人 处理 3D 点 云 数据 ,客户 端 用 于 接收 和 浓 染 3D 点 云 数 
据 。 服 务 端 和 客户 端 之 间 通 过 局 域 网 络 连接 。 软 件 开 发 环境 为 Microsoft Visual Studio 
2010 ,将 Kinect 放 在 桌面 上 ,整体 实验 设备 安装 图 如 图 10. 36 所 示 。 


服务 端 电脑 显示 器 


Kinect 传 感 器 


客户 端 电脑 


ee 
四 
和 


服务 端 电脑 


图 10.36 整体 实验 设备 安装 图 


2. 基于 人 脸 检测 压缩 图 像 实验 

在 带 通 滤波 器 中 ,将 Z 轴 视 为 特定 字段 ,其 中 xi =0.0 和 ,二 1.0。 因 此 ,如 果 使 用 带 通 
滤波 器 来 处 理 点 云 数据 , 则 仅 保 留 深度 为 0 一 lm 的 点 。 在 统计 学 滤波 器 中 ,Y= 二 20 和 stdiu 二 
1.0。 在 RANSAC 算 法 中 ,将 一 个 平面 模型 作为 特定 模型 ,并 将 阔 值 选择 为 0. 1。 

实验 中 使 用 OpenNI 接口 从 Kinect 传感器 获取 3D 图 像 。 如 图 10. 37 所 示 , 对 于 HRI,3D 
图 像 中 的 大 量 信息 是 不 必要 的 ,并 且 这 些 信息 会 增加 网 络 负担 。 显 然 ,3D 点 云 数 据 非常 大 。 


如 果 通 过 网 络 直接 传输 3D 点 云 数据 ,将 导致 网 络 拥塞 
更 为 严重 。 此 外 ,对 于 HRI,3D 点 云 数据 中 的 大 量 信 
息 是 不 必要 的 。 因 此 ,应 该 使 用 一 些 算法 来 处 理 3D 点 
云 数 据 。 为 了 减少 传输 时 延 , 在 两 个 I 帧 之 间 发 送 三 
帧 P 帧 。 首 先 通过 HAAR 分 类 器 获取 人 脸 的 粗略 轮 
廓 ,并 根据 立轴 (垂直 方向 ) 的 值 将 3D 人 脸 信息 划分 
成 多 个 切片 。 然 后 ,以 20(4d, =20) 的 步 长 遍历 切片 ， 
并 计算 三 角形 的 高 度 , 这 将 获得 鼻尖 的 粗糙 区 域 。 接 
下 来 ,获取 最 终 的 鼻尖 位 置 ,并 且 根 据 以 鼻尖 为 中 心 的 
球体 获取 3D 人 脸 的 最 终 信 息 。 本 实验 选择 半径 为 
-一 80mm。 

为 了 增强 互动 过 程 中 的 沉浸 感 ,前 台 机 器 人 的 上 
半身 是 来 自 操作 室 的 操作 者 上 半身 的 3D 图像。 显然 ， 
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图 10.37 直接 由 Kinect 传感器 
获取 的 3D 图 像 


为 了 更 加 通 真 地 显示 人 体 虚 拟 信 息 ,3D 图 像 中 的 背景 信息 应 该 去 除 , 然 后 , 带 通 滤波 右 用 来 去 
除 3D 图 像 的 背景 信息 。 使 用 图 像 处 理 算法 后 ,得 到 的 图 像 如 图 10. 38 有 由 图 10. 38(a) 
可 以 看 到 ,已 经 删除 了 3D 图 像 的 背景 信息 。 因 为 3D 图 像 中 的 大 量 信息 是 背景 信息 ,显然 点 
云 数据 的 数量 急剧 减少 。 由 于 目前 3D 点 云 数 据 中 存在 一 些 异 常 值 ,使 得 3D 图 像 数据 集 的 密 
度 不 均匀 。 统 计 学 滤波 器 可 以 通过 计算 输入 数据 集中 每 个 点 的 相 邻 距离 来 去 除 稀 玖 异常 值 ， 


使 数据 集 的 密度 变 得 更 加 均匀 ,结果 如 图 10. 38(b) 所 示 。 


(a) 用 带 通 滤波 器 处 理 过 的 3D 图 像 (b) 用 统计 学 滤波 器 处 理 过 的 3D 图 像 


(c) 用 RANSAC 算 法 处 理 过 的 3D 图 像 (d) 利用 人 脸 检 测 ， 分 割 出 人 脸 信息 
图 10.38 图 像 处 理 算法 得 到 的 图 像 
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能 够 去 除 3D 图 像 的 背景 和 异常 值 ,并 使 3D 图 像 数 据 集 的 密度 更 加 均匀 ,但 仍然 有 一 些 
3D 图 像 小 块 对 人 机 交互 是 不 必要 的 。 为 了 减少 传输 数据 量 并 减少 传输 时 间 延 迟 ,使 用 分 割 算 
法 来 处 理 3D 点 云 数 据 ,从 而 消除 一 些 不 必要 的 3D 图 像 小 块 信息 。 在 图 10. 38(b) 中 ,一 些小 块 
3D 图 像 对 于 人 机 交互 是 不 必要 的 ,如 图 中 圈 出 的 小 块 。 如 图 10. 38(Cc) 所 示 ,使 用 RANSAC 算法 
去 除 对 于 人 机 交互 不 必要 的 一 些小 块 3D 图 像 。 

Kinect 传感器 能 够 在 1s 内 收集 30 帧 3D 图 像 ,如 果 通 过 网 络 直接 传输 完整 的 3D 图 像 ， 
将 浪费 网 络 资源 ,因为 两 帧 图 像 之 间 的 相似 性 非常 高 。 因 此 ,将 在 两 个 I 帧 之 间 发 送 3 帧 P 
帧 。 因 为 3D 图 像 的 两 帧 之 间 的 主要 变化 发 生 在 人 脸 区 域 , 所 以 将 面部 信息 视 为 P 帧 的 信息 ， 
如 图 10. 38(d) 所 示 。 最 后 ,使 用 TCP 协议 传输 点 云 数据。 

3D 图 像 处 理 过 程 中 图 像 大 小 和 下 降 比 例 变 化 如 表 10. 2 所 示 。 表 10.2 中 ,原始 3D 点 云 
的 个 数 为 307200(640X480) 个 。 通 过 带 通 滤波 器 处 理 后 ,3D 点 云 个 数 急剧 下 降 至 67676 个 。 
然后 ,通过 统计 学 滤波 器 和 RANSAC 算法 处 理 ,3D 点 云 个 数 变 为 52343 个 。 最 后 ,P 帧 点 云 
个 数 为 4520 个 。 当 使 用 带 通 滤波 器 处 理 3D 图 像 时 ,点 云 数据 下 降 的 比例 达到 0.7797., 整 个 
流程 的 下 降 比 例 达 到 0. 8296 (如 果 只 考虑 了 帧 图 像 的 信息 量 , 则 为 0. 9853)。 

关于 处 理 和 传输 的 时 间 延 迟 , 通 过 基于 面部 检测 的 压缩 算法 处 理 图 像 ,花费 3373. 125ms 
处 理 和 传输 4 帧 3D 图像 (包括 1 帧 工 帧 信息 和 3 帧 P 帧 信息 ) ,平均 时 间 延 迟 843.281ms。 压 
缩 比 等 于 平均 传输 帧 的 大 小 除 以 原始 图 像 大 小 ,最 终 压 缩 比 为 5. 3632。 


表 10.2 3D 图 像 处 理 过 程 中 图 像 大 小 和 下 降 比 例 变化 


3D 图 像 处 理 过 程 图 像 大 小 
307200 


67676 


58614 
52343 


4520 


下 降 比 例 


原 图 
带 通 滤波 器 处 理 后 
统计 学 滤波 器 处 理 后 
RANSAC 分 割 算法 处 理 后 
提取 人 脸 信 息 后 


3. 基于 八 叉 树 结构 压缩 图 像 实验 

为 了 体现 基于 人 脸 检 测算 法 的 压缩 算法 的 优点 ,本 节 做 了 比较 实验 。 实 验 利 用 八 又 树 结 
构 直 接 压 缩 了 Kinect 传感器 抓 取 的 3D 点 云 数据 。 

八 叉 树 数 据 结 构 适 用 于 稀 朴 的 3D 点 云 , 每 个 分 支 节 点 表示 某 个 立方 体 。 八 叉 树 分 割 的 方 
式 是 将 立方 体 分 割 为 8 个 小 立方 体 ,然后 继续 划分 小 立方 体 , 直 到 小 立方 体 不 可 分 离 。 一 般 来 
说 ,每 个 节点 对 应 一 个 存储 器 中 的 一 个 对 象 , 它 保存 着 8 个 指向 其 他 节点 的 指针 。 如 果 节 点 具 - 
有 少 于 8 个 子 节点 ,那么 其 中 至 少 一 个 是 NULL 指针 。 八 叉 树 数据 结构 如 图 10. 39 所 示 。 


IOOOOOOOOHNIOMNNND 
图 10.39 八 叉 树 数据 结构 图 
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使 用 基于 八 又 树 的 压缩 算法 来 压缩 由 Kinect 传感器 抓 取 的 3D 点 云 数据 。 如 通过 Kinect 
传感器 获取 3D 图 像 ,然后 直接 压缩 3D 图 像 , 则 无 须 经 过 任何 算法 处 理 ( 如 图 10. 40(a) 所 示 )， 
如 去 除 3D 图 像 中 的 背景 和 异常 值 。 通 过 基于 八 又 树 的 压缩 算法 处 理 得 到 的 图 像 如 图 10. 40(b) 
所 示 。 


(a) 基于 八 叉 树 压缩 处 理 前 的 3D 图 像 
图 10. 40 ”基于 八 又 树 的 压缩 算法 的 效果 图 


(b) 基于 八 叉 树 压缩 处 理 后 的 3D 图 像 


在 处 理 和 传输 的 时 间 延 迟 方面 , 耗 时 8652ms 处 理 和 传输 4 帧 3D 图 像 , 基 于 八 又 树 的 压 
缩 算法 的 平均 时 间 延 迟 2163ms。 然 而 ,压缩 比 为 10.544( 压 缩 比 等 于 传输 帧 的 大 小 除 以 原始 
图 像 的 大 小 ) ,因此 ,该 算法 在 计算 方面 显然 非常 复杂 。 

4. 实验 结果 

本 节 使 用 处 理 和 传输 的 时 间 延 迟 、 压 缩 比 和 失真 率 来 评估 算法 的 性 能 。 通 过 肉眼 观察 最 
终结 果 得 到 失真 率 ,从 实验 结果 来 看 ,该 方法 的 失真 率 将 高 于 基于 八 叉 树 的 压缩 算法 的 失真 
率 。 然 而 ,该 方法 的 压缩 率 几 乎 与 基于 八 又 树 压 缩 的 压缩 率 相 同时 ,方法 的 处 理 和 传输 的 时 
间 延 迟 将 远 远 小 于 基于 八 又 树 的 压缩 算法 的 处 理 和 传输 的 时 间 延 迟 。 对 比 结果 如 表 10. 3 
所 示 。 


表 10.3 基于 人 脸 识别 压缩 算法 和 基于 八 又 树 压 缩 算 法 对 比 结果 


方法 基于 人 脸 识别 压缩 算法 基于 八 又 树 压 缩 算法 
时 间 延 时 843. 281ms 2163ms 
压缩 比 5. 3632 10. 544 
失真 率 相对 高 相对 低 
计算 量 相对 简单 相对 复杂 


分 析 对 比 结果 可 知 , 该 处 理 方法 能 够 保证 网 络 流畅 ,减少 处 理 和 传送 的 时 间 延 迟 。 这 种 方 
法 的 压缩 比 相 当 低 ,失真 率 相对 较 高 ,但 处 理 和 传输 的 时 间 延 迟 将 大 大 缩短 。 比 较 延 时 时 间 和 
压缩 比 , 基 于 人 脸 识别 的 压缩 算法 和 基于 八 又 树 的 压缩 算法 的 压缩 比 相差 不 大 ,但 是 基于 八 又 
树 的 压缩 算法 的 延 时 时 间 远 远大 于 基于 人 脸 识 别 的 压缩 算法 的 延 时 时 间 ,可 见 , 基 于 八 又 树 的 
压缩 算法 的 计算 量 特别 大 。 另 外 ,在 远程 操作 室 的 操作 员 的 上 半身 的 3D 图 像 用 于 前 台 机 器 
人 的 上 半身 ,该 应 用 系统 不 需要 非常 低 的 失真 率 。 因 此 ,根据 时 间 延 迟 、 压 缩 比 和 失真 率 的 比 
较 结果 ,本 方法 的 性 能 更 好 。 
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10.4 生物 反馈 增强 型 多 媒体 游戏 的 开发 


作为 一 种 前 沿 技术 ,生物 反馈 已 广泛 用 于 心理 治疗 。 本 节 将 介绍 一 种 基于 生物 反馈 技术 
的 多 媒体 游戏 的 设计 , 主要 包括 两 部 分 : 脑 机 接口 (BCI) 和 多 媒体 游戏 设计 。 

在 生物 反馈 技术 中 , 脑 机 接口 (Brain Computer Interface,BCI) 是 最 流行 和 最 成 熟 的 方法 
之 一 , 它 可 以 最 直接 地 反映 人 的 情绪 。 此 外 ,BCI 提供 了 大 脑 和 外 围 设备 之 间 的 直接 通信 途 
径 。 通 过 BCI, 我 们 不 需要 先 用 手 或 腿 等 肢体 语言 表达 我 们 的 意见 。 来 自 人 的 大 脑 的 命令 可 
以 直接 执行 游戏 ,驾驶 轮椅 在 没有 手 或 腿 参与 的 情况 下 控制 机 器 人 ; 通过 相关 设备 ,残疾 人 可 
以 不 需要 他 们 的 “ 腿 ” 或 “ 手 ” 来 站 起 来 .走路 和 进行 较为 复杂 的 操纵 。 基 于 上 述 原因 ,本 节 将 在 
研究 中 采用 BCI 进行 生物 反馈 设计 视频 游戏 。 

电脑 游戏 在 群众 尤其 是 年 轻 人 中 非常 受 欢 迎 。 因 此 ,可 以 将 生物 反馈 技术 与 一 些 有 趣 的 
游戏 相 结合 ,以 提高 其 效果 。 有 许多 软件 可 用 于 游戏 开发 ,例如 ,MATLAB、Visual Studio( 即 
VS) 和 QTCreator。 其 中 ,MATLAB 具有 强大 的 计算 能 力 ,VS 拥有 大 量 的 程序 库 。 但 对 于 界 
面 开 发 ,QTCreator 是 最 佳 选择 。 首 先 , 它 具有 强大 的 图 形 视图 框架 ,这 有 助 于 有 效 地 处 理 游 
戏 中 图 形 项 之 间 的 复杂 关系 。 其 次 , 它 在 跨 平台 上 具有 独特 的 能 力 ,在 未 来 可 以 轻松 地 将 游戏 
扩展 到 其 他 平台 。 因 此 ,选择 QTCreator 作为 游戏 的 开发 软件 。 

生物 反馈 增强 型 多 媒体 游戏 的 系统 总 体 结构 如 图 10. 41 所 示 。 首 先 ,通过 Neuroscan 装 
置 收集 脑 电 图 (electroencephalogram ,EEG) 信 号 并 发 送 到 SCAN4.5(SCAN4.5 是 Neuroscan 
的 匹配 软件 )。SCAN4. 5 提供 了 一 个 TCP 端口 ,如 果 已 经 完成 了 MATLAB TCP 客户 端的 
设置 ,MATLAB 可 以 实时 获取 EEG 数据 。 在 MATLAB 中 ,可 以 处 理 原 始 EEG 数据 ,提取 
特征 ,然后 使 用 其 强大 的 计算 功能 对 它们 进行 分 类 。 之 后 ,通过 UDP 套 接 字 将 分 类 结果 发 送 
到 QTCreator ,在 QTCreator 中 设计 的 游戏 过 程 将 响应 这 些 结果 。 在 以 下 部 分 中 ,将 依次 呈现 
如 图 10. 41 所 示 的 系统 。 


生物 反馈 游戏 系统 


QTCreator 


TCP 传 输 

脑 电 0 
数据 最， 
CE 


[he SCAN4.5 MATLAB 
te i 
NEUROSCAN 


图 10.41 系统 总 体 结构 
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10.4.1 BCI 的 设计 

1. ERS/ERD 现象 和 信号 收集 

虽然 大 脑 以 复杂 的 方式 运作 ,但 仍 可 通过 很 多 线索 推断 其 思想 。 实 际 上 ,人 类 大 脑 由 数 十 
亿 个 神经 细胞 组 成 ,它们 相互 沟通 ,不 同 的 意图 将 导致 大 脑 中 神经 细胞 信号 的 不 同 潜在 分 
布 。 例 如 , 当 想 要 移动 身体 的 某 些 部 分 时 ,大 
脑 中 特定 位 置 上 EEG 信号 的 能 量 水 平 将 降 。“ 
低 ,这 称 为 事件 相关 的 去 同步 (event related 60 
desynchronization, ERD) 现象 。 相 反 地 , 当 停 50 

止 想象 移动 时 , EEG 信号 的 能 量 水 平 将 上 升 ， 40 

-这 称 为 事件 相 关 同 : 步 (event related 到 
synchronization, ERS) 现象 。 这 两 种 现象 在 a 
节律 和 B 节律 中 表现 明显 ,如 图 10.42 所 示 。 
根据 能 量 水 平 的 不 同 表现 ,可 以 将 脑 电 信号 分 0 
为 两 类 一 一 想象 和 放松 ,这 种 技术 称 为 运动 想 0 


0 8 10 15 20 


30 


象 ,并 且 有 许多 方法 可 以 对 运动 想象 中 的 不 同 频率 
模式 进行 分 类 。 对 于 EEG 信号 采集 来 说 ， 图 10. 42 ”ERS/ERD 现象 


Neuroscan 具有 高 分 辨 率 、 高速 ,多 通道 的 特 

点 ,具有 40 个 传感器 ,是 很 好 的 信号 放大 器 。 这 里 用 它 来 收集 大 脑 中 的 原始 EEG 数据 。 在 计 
算 机 中 ,原始 数据 必须 首先 由 名 为 SCAN4. 5 的 Neuroscan 匹配 软件 接收 。 然 后 发 送 到 
MATLAB 以 通过 TCP 套 接 字 进 行 处 理 。 

2. 预 处 理 和 特征 提取 算法 

由 于 在 原始 EEG 信号 中 混合 有 许多 噪声 信号 ,因此 需要 进行 预 处 理 。 最 重要 的 任务 是 滤 
除 a 节律 和 有 节律 中 的 信号 ,其 中 ERS 和 ERD 现象 表现 明显 。 预 处 理 有 两 种 方法 。 首 先 ,可 
以 在 SCAN4. 5 中 设置 内 置 滤 波 器 的 低 通 频率 和 高 通 频率 。 其 次 ,还 可 以 使 用 MATLAB 的 
过 滤 函 数 。 

预 处 理 后 ,将 采用 通用 空间 模式 (common spatial pattern,CSP) 算 法 提取 信和 号 的 特征 。 该 
算法 的 主要 思想 是 构造 一 个 变换 器 W', 它 可 以 将 一 种 信号 的 方差 最 大 化 ,并 将 另 一 种 信号 的 
方差 最 小 化 ,以 便 更 明显 地 区 分 它们 。 

变换 器 W' 可 以 按照 以 下 步骤 求 取 。 

首先 计算 EEG 信号 矩阵 的 协 方差 。 计 算 表 达 式 如 下 : 

J 

trace(EE) 
其 中 ,和 矩阵 瑟 ER”” 表示 EEG 信号 的 数据 ,其 中 N 表示 电极 的 数量 ,T 表示 集合 中 的 采样 
点 的 数量 。“trace()” 表 示 计 算 和 矩阵 的 对 角 元 素 之 和 。 使 用 式 (10. 55) ,可 以 计算 ”次 左手 运 
动 想象 和 左手 放松 数据 的 协 方差 ,分 别 用 C,, 和 C,; 表示 。 它 们 平均 值 的 计算 如 下 : 


410..55) 


C, = 0 (10756) 


C,= 2C,, (10. 57) 


其 中 ,C, 表示 所 有 Cu 的 平均 值 ,C, 表示 所 有 C,; 的 平均 值 。 然 后 ,应 该 计算 混合 空间 的 协 
方差 : 
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C.=C,+C., (10. 58) 
式 中 ,C。 表示 混合 空间 的 协 方差 。 
其 次 ,对 矩阵 C. 进行 分 解 : 
C, =U, x*A. xUL (10. 59) 
可 以 得 到 两 个 矩 阵 , 即 特征 向 量 矩 阵 U。 和 和 矩阵 C。 的 特征 值 矩阵 4。。 
最 后 ,对 和 矩阵 C. 进行 白化 变换 : 


本 (10. 60) 
S,=PxC,*P! (10. 61) 
S$, =P xC,*P! (10. 62) 
经 过 以 上 白化 处 理 后 ,和 矩阵 $S, 和 S, 的 特征 向 量 相 同 , 再 对 其 进行 特征 值 分 解 : 
S,=BxA,xB' (10. 63) 
S,=B*xA,x*xB!' (10. 64) 
则 所 需 的 变换 器 W 为 : 
环 三 全 区 画 - (10. 65) 
3. 分 类 算法 


对 于 分 类 ,采用 线性 判别 分 析 (linear discriminant analysis,LDA) 算 法 。 它 是 一 个 简单 而 
稳定 的 分 类 器 ,基于 已 知 样本 构建 分 类 标准 ,然后 根据 该 标准 对 新 样本 进行 分 类 。 
LDA 的 表达 如 下 : 
D(z)=b+w .x (10. 66) 
其 中 ,“b” 表 示 新 样本 ,“b” 和 “w” 是 从 已 知 样本 计算 得 到 的 参数 ,“D (zx)” 是 分 类 的 结果 。 如 果 
D 二 0, 则 表示 zx 是 其 中 一 种 信号 ; 如 果 D 二 0, 则 表示 x 是 另 一 种 信号。 


4. 离线 BCI 的 实验 
基于 上 面 提 到 的 算法 ,现在 可 以 建立 离线 BCI, 训 练 模式 如 图 10. 43 所 示 。 在 第 一 阶段 ， 
受 试 者 用 10s 进行 准备 ,然后 , 受 试 者 进行 左 总 循环 数目 可 调 


手 的 运动 想象 ,并 在 下 一 个 时 间 间 隔 停止 想 
象 ; 接着 受 试 者 再 进行 右手 的 运动 想象 ,之 后 
停止 运动 ,重复 上 述 过 程 。 

在 建立 训练 模式 之 后 ,关于 如 何 保证 离线 
EEG 数据 和 训练 间隔 之 间 的 同步 问题 。 简 单 
来 说 ,在 完成 训练 实验 并 获得 记录 EEG 数据 
的 离线 文件 后 ,还 不 知道 每 个 间隔 与 其 EEG 可 恋 时 间 间 隔 
数据 之 间 的 确切 对 应 关系 。 为 了 解决 这 个 问 图 10.43 “训练 模式 
题 ,在 QTCreator 中 创建 了 一 个 训练 窗口 ,如 
图 10. 44 所 示 。 

在 这 个 窗口 中 可 以 设置 训练 的 参数 ,如 时 间 间 隔 、 循 环 索引 等 。 更 重要 的 是 , 它 可 以 与 
SCAN4. 5 进行 通信 。 如 果 单 击 “ 开 始 实验 ”按钮 , 它 会 向 SCAN4. 5 发 送 命 令 , 让 它 开 始 收集 
和 记录 EEG 数据 。 完 成 训练 后 , 它 会 向 SCAN4. 5 发 送 命 令 并 让 它 停止 收集 ,这 保证 了 数据 
文件 和 训练 模式 在 开始 和 结束 时 刻 之 间 的 同步 。 然 后 ,可 以 根据 训练 前 设 定 的 时 间 间 隔 、 周 期 
指数 和 采样 率 计 算 每 个 区 间 的 EEG 数据 。 

完成 所 有 这 些 准备 工作 后 ,实验 对 象 只 需 坐 在 显示 器 前 面 , 然 后 查看 培训 窗口 ,按照 窗口 


dt ay 
泪 晓 车 小 了 


左 
手 
运 
动 
想 
象 
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case 


图 10.44 训练 窗口 (屏幕 快照 ) 


给 出 的 说 明 进 行 操作 。 经 过 实验 ,将 获得 一 个 离线 文件 。 从 文件 中 可 以 获得 EEG 数据 ,并 通 
过 上 述 算法 处 理 它们 。 

5. 实验 结果 

实验 完成 后 ,得 到 一 个 离线 EEG 数据 文件 。 首 先 从 文件 中 加 载 原 始 数据 ,并 对 其 进行 预 
处 理 , 以 滤 除 频率 为 8 一 12Hz 的 特定 频率 ; 并 对 原始 数据 和 过 滤 数 据 进行 比较 ,如 图 10. 45 所 
示 。 从 时 域 中 几乎 看 不 出 它们 之 间 的 区 别 。 但 是 如 果 在 频谱 中 对 它们 进行 可 视 化 ,可 以 更 容 
易 地 区 分 它们 ,如 图 10. 46 所 示 。 然 后 ,可 以 对 过 滤 数 据 进 行 CSP 转换 ,并 提取 特征 、 对 其 进 
行 分 类 ,实验 的 分 类 结果 如 图 10. 47 所 示 ,其 中 ,小 圆圈 表示 运动 想象 ,小 又 表示 放松 状态 。 如 
图 10. 47 所 示 ,这些 算法 的 准确 率 高 达 81. 3%。 


原始 数据 


过 滤 数 据 


0 0.5 1.0 1.5 2.0 2.5 3.0 
X104 


图 10.45 原始 EEG 数据 和 过 滤 数 据 的 比较 
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oi 一 一 原始 数据 
: -一 一 - 过 滤 数 据 


图 10.46 原始 数据 和 过 滤 数 据 的 频谱 


二 六 运动 想象 VS 放松 状态 


1.0 1.5 2.0 2.5 3.0 3.5 4.0 4.5 
图 10.47 两 个 信号 的 分 类 


10.4.2 多 媒体 游戏 的 设计 

1. 游戏 规则 的 设计 

多 媒体 游戏 的 界面 如 图 10. 48 所 示 。 用 户 汽 车 .障碍 车 和 背景 的 图 像 都 是 从 互联 网 上 下 
载 的 。 在 这 场 比赛 的 场景 中 有 4 个 车 道 。 场 景 底部 的 红 
色 汽车 是 用 户 的 车 ,其 他 汽车 则 是 障碍 车 ,它们 将 随机 出 
现在 场景 中 。 开 始 时 ,用 户 车 的 速度 为 0, 然 后 它 会 自动 
加 速 并 赶 上 障碍 车 。 如 果 玩 家 来 不 及 改变 车 道 , 它 将 与 它 
们 发 生 碰 撞 。 然 后 它 的 速度 将 回 到 0, 并 且 必 须 再 次 
加 速 。 

已 经 定义 了 两 个 关于 游戏 中 碰撞 的 重要 事件 。 第 一 
个 是 上 面 提 到 的 碰撞 事件 。 当 用 户 的 汽车 与 障碍 车 的 后 
部 发 生 碰 撞 时 ,就 会 发 生 这 种 情况 。 在 这 种 情况 下 ,用户 
的 车 将 停止 ,并 且 障 碍 车 将 被 击 中 ,向 前 飞行 一 段 距离 。 
第 二 个 是 切入 事件 。 当 用 户 的 车 与 障碍 车 平行 运行 ,然后 时 
改变 车 道 至 障碍 车 的 车 道 时 ,就 会 发 生 这 种 情况 。 在 这 种 。 图 10.48 多 媒体 游戏 的 界面 
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情况 下 ,障碍 车 必须 让 路 给 用 户 或 被 摧毁 ,并 且 用 户 车 的 速度 将 增加 。 第 二 个 事件 将 增加 这 个 
游戏 的 趣味 性 。 

2. 游戏 框架 的 设计 

确定 游戏 规则 后 ,开始 设计 游戏 程序 的 框架 。QTCreator 提供 了 一 个 强大 的 游戏 设计 图 
形 视图 框架 。 在 其 框架 中 ,主要 有 -3 个 重要 组 件 , 即 QGraphicsScene、QGraphicsView 和 
QGraphicsItem。 QGraphicsScene 类 似 于 一 个 空房 子 , 可 以 添加 很 多 “家 具 ”(QGraphicsItem)。 它 
不 仅仅 是 一 个 简单 的 房子 。 它 知道 每 个 成 员 的 坐标 、 轮 廓 、 运 行 状态 等 。 它 还 配备 了 “广播 系 
统 ”, 一 旦 发 生 一 些 重要 事件 ,例如 碰撞 和 单 击 鼠 标 , 它 将 通知 所 有 成 员 或 其 中 的 一 些 特定 成 
员 。QGraphicsView 类 似 于 房子 的 窗户 ,通过 此 窗口 ,可 
_ 以 在 本 地 或 全 局 观察 QGraphicsScene 中 的 项 目 ,还 可 以 障碍 物 汽车 链表 
设置 此 窗口 的 大 小 和 透明 度 。QGraphicsItem 是 这 栋 房 
子 里 的 家 具 。 例 如 ,在 这 个 游戏 中 ,用 户 的 汽车 、 障 碍 汽车 
和 背景 图 片 都 是 QGraphicsItem。 它 为 提供 了 一 些 基本 功 
能 ,用 于 绘制 项 目的 形状 、 移 动 项 目 、 检 测 边缘 和 碰撞 等 。 
基于 此 框架 ,将 游戏 场景 作为 框架 的 中 心 ,因为 它 可 以 轻 
松 管理 其 中 包含 的 所 有 图 形 项 目 。 图 形 视 图 可 以 被 视 为 
窗口 系统 中 的 窗口 ,用 于 从 其 视角 观察 游戏 场景 ; 障碍 车 
被 设计 成 链表 结构 以 便 更 好 地 管理 ; 背景 图 片 也 是 场景 的 
一 个 项 目 , 它 会 根据 用 户 汽车 的 速度 快速 或 缓慢 地 在 屏幕 
上 移动 ,这 样 就 会 使 用 户 产 生 一 种 “幻觉 ”, 即 汽车 前 进 , 实 用 户 汽车 
际 上 始终 停留 在 场景 的 底部 。 多 媒体 游戏 的 框架 如 图 10.49 多 媒体 游戏 的 框架 
图 10. 49 所 示 。 

3. 基本 游戏 的 实现 

为 了 可 视 化 游戏 设计 ,需要 在 场景 中 绘制 用 户 的 汽车 .障碍 车 和 背景 图 片 等 图 形 项 目 。 在 
这 一 步 中 ,使 用 名 为 paint() 的 QGraphicsItem 中 的 成 员 函 数 。 使 用 此 功能 时 ,可 以 使 用 QT 
的 paint() 绘 制 任何 形状 或 加 载 图 片 来 绘制 项 目 。 在 下 一 步 中 ,使 用 moveby(Cz,y) 函 数 来 移 
动 项 目 。 函 数 括号 中 的 参数 表示 项 目 在 zx 和 y 方向 上 的 位 移 。 要 实时 自动 移动 项 目 , 可 以 使 
用 QT 的 计时 器 。QT 计时 器 的 基本 机 制 如 下 : 首先 设置 计时 器 的 间隔 并 运行 ,之 后 定时 器 将 
运行 , 当 它 完成 一 个 间隔 时 , 它 将 执行 超时 功能 ,然后 重复 开始 下 一 个 间隔 。 所 以 需要 设置 时 
间 间 隔 并 将 “moveby()” 函 数 写 入 定时 咒 的 超时 功能 。 

接 下 来 ,项 目 将 以 特定 速度 自动 在 屏幕 中 移动 。 如 果 想 要 改变 速度 ,可 以 改变 定时 器 的 间 
隔 或 者 moveby() 函数 中 的 参数 。 然 后 考虑 游戏 的 速度 机 制 。 游 戏 中 主要 有 两 种 速度 一 一 用 
户 汽车 的 速度 和 障碍 车 的 速度 。 对 于 障碍 车 ,它们 根据 其 速度 在 屏幕 中 向 前 或 向 后 移动 。 如 
果 它 们 在 视图 之 外 运行 ,将 会 消失 。 但 是 ,对 于 用 户 的 汽车 , 它 不 能 在 屏幕 上 消失 。 实 际 上 它 
总 是 停留 在 场景 的 底部 。 这 里 不 是 移动 用 户 的 汽车 ,而 是 沿 相反 方向 移动 背景 图 片 ,以 产生 用 
户 汽车 正在 向 前 跑 的 错觉 。 但 如 果 在 游戏 过 程 中 继续 移动 背景 图 片 ,就 需要 找到 一 张 长 图 ,这 
很 难 找 到 ,并 且 将 限制 总 游戏 时 间 ,因为 当 图 片 结 束 时 ,游戏 必须 停止 ,所 以 在 背景 电影 中 制作 
一 个 循环 。 首 先 , 找 到 一 张 背景 图 片 并 将 副本 粘贴 到 它 的 项 部。 然后 让 视 口 沿 着 新 图 片上 升 。 
一 旦 视 口 的 下 边缘 线 到 达 初 始 背 景 图 片 的 上 边缘 线 , 它 将 返回 到 初始 背景 图 片 的 下 边缘 线 ,并 
继续 此 循环 。 

现在 介绍 如 何 计算 背景 和 障碍 物 相 对 于 屏幕 的 速度 ,公式 如 下 ， 
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VW (10. 67) 
Ve eV (10. 68) 
其 中 ,Vb。 和 V。。 表示 背景 和 障碍 车 相对 于 屏幕 的 速度 ,它们 是 屏幕 中 的 实际 移动 速度 ,wu。 和 
Vs 是 理论 上 用 户 汽 车 和 障碍 车 的 速度 。 接 下 来 ,将 这 些 速度 转换 为 QT 计时 器 的 时 间 间 隔 。 


公式 如 下 : 
业 
l=aXT (10. 69) 
其 中 ,I 是 计时 器 的 间隔 ,V 是 物体 相对 于 屏幕 的 速度 ,a 是 可 变 系数 。 通 过 增加 a, 可 以 增加 
游戏 的 难度 。 


4. 碰撞 检测 和 多 媒体 的 实现 

使 用 QTCreator 作为 开发 平台 的 一 个 优点 是 它 具 有 处 理 图 形 项 之 间 冲 突 的 有 效 机 制 。 
一 旦 将 诸如 障碍 车 之 类 的 物品 添加 到 游戏 场景 中 ,QT 将 自动 监督 其 声明 。 如 果 它 与 场景 中 
的 其 他 项 发 生 碰撞 ,QT 将 立即 执行 碰撞 处 理 功 能 。 实 际 上 ,这 些 功能 是 QT 的 “ 插 槽 ”功能 ， 
它们 可 以 与 “信号 ”功能 相关 联 , 以 处 理 信号 事件 ,例如 碰撞 和 键盘 输入 。 在 这 种 情况 下 ,事件 
是 碰撞 。 为 了 实现 碰撞 检测 和 管理 ,需要 将 项 目 添加 到 场景 中 ,并 将 碰撞 管理 集成 到 
QTCreator 中 的 “ 插 槽 ?功能 中 ,无 须 担 心 如 何 检测 项 目 是 否 与 其 他 项 目 发 生 冲 突 , 它 将 自动 实 
施 。 因 此 ,这 种 机 制 使 得 程序 更 具 可 读 性 ,也 更 简单 。 

为 了 获得 更 好 的 游戏 体验 ,还 可 以 为 游戏 添加 一 些 背景 音乐 。 为 了 实现 它 , 可 以 使 用 另 一 
个 名 为 Phonon 的 QT 类 。 首 先 , 创 建 一 个 新 的 Phonon 类 变 体 。 然 后 设置 输出 方式 和 背景 音 
乐 的 路 径 。 最 后 ,命令 播放 音乐 。 

5. 使 用 生物 反馈 控制 游戏 

本 节 将 通过 使 用 生物 反馈 技术 完成 可 以 玩 多 媒体 游戏 的 目标 。 

前 述 通过 一 些 软件 完成 了 BCI 和 多 媒体 游戏 的 设计 。 现 在 的 核心 问题 是 如 何 将 这 些 软 
件 ( 即 MATLAB,QTCreator 和 SCAN4. 5) 组 合 在 一 起 。 其 中 ,SCAN4. 5 用 于 从 Neuroscan 
中 收集 EEG 数据 ,MATLAB 用 于 数据 处 理 和 分 类 ,QTCreator 用 于 游戏 设计 。 如 图 10. 41 
所 示 ,EEG 数据 首先 从 SCAN4. 5 发 送 到 MATLAB, 将 分 类 结果 从 MATLAB 发 送 到 
QTCreator。 因 此 首要 任务 是 建立 这 两 种 沟通 途径 。 

这 里 SCAN4. 5 提供 了 一 个 实时 获取 EEG 数据 的 TCP 端口 。 在 SCAN4. 5 的 规范 中 ,有 
TCP 数据 包 的 详细 描述 ,数据 包 的 结构 如 图 10. 50 所 示 。 从 图 中 可 以 看 出 ,数据 包 由 两 部 分 
组 成 , 即 标题 部 分 和 数据 部 分 。 标 题 中 有 4 个 成 员 : BODY SIZE 表示 数据 部 分 将 在 此 数据 包 
中 拥有 多 少 字 节 ; 其 他 3 个 具有 不 同 值 的 成 员 的 组 合 ,表示 MATLAB 和 SCAN4.5 之 间 的 不 
同 命令 ,可 以 使 用 这 些 命令 使 SCAN4.5 开始 收集 数据 发送 数据 .停止 恢 集 等 。 因 此 ,这 里 的 
工作 是 根据 SCAN4. 5 提供 的 协议 编写 MATLAB TCP 客户 端 程序 。 在 编写 客户 端 程序 时 ， 
必须 考虑 两 个 重要 问题 。 一 个 是 如 何 准确 地 接收 和 分 析 标 题 , 此 问题 将 在 出 错时 导致 一 系列 
错误 ,因为 TCP 的 数据 流 是 连续 的 。 另 一 个 是 如 何 匹配 发 送 方 和 接收 方 之 间 的 速度 ,因为 无 
法 太 快 或 太 慢 地 接收 数据 。 要 解决 这 个 问题 ,最 好 使 用 MATLAB 计时 器 , 找 出 最 佳 的 计时 器 
间隔 。 

为 了 建立 QTCreator 和 MATLAB 之 间 的 通信 通道 ,有 几 种 方法 。 一 种 是 使 用 
MATLAB 引擎 ,该 方法 稳定 但 速度 太 慢 , 会 破坏 用 户 在 游戏 中 的 体验 。 另 一 种 选择 是 使 用 更 
快 的 动态 链接 库 ,这 种 方法 非常 依赖 于 MATLAB 和 QTCreator 的 版 本 ; 如 果 更 改 这 些 软件 
的 版 本 ,可 能 会 导致 很 多 错误 。 这 里 采用 一 种 新 方法 ,使 用 UDP 套 接 字 作为 这 两 个 软件 之 间 


第 10 章 人 机 交互 | 加 279 


Ss 


数据 包 首部 (标题 部 分 ) 数据 


数据 部 分 
大 小 为 body size 个 字 节 


寸 世 瑟 
3urns qdI lsyoed 
寸 攻 习 
LSINOTY 
4ZIS AdOg 


图 10.50 ”TCP 数据 包 的 结构 


”的 通道 。 首 先 , 它 稳定 而 快速 ,完全 不 依赖 于 版 本 或 操作 系统 。 两 个 软件 进程 可 以 在 系统 中 并 
行 运行 而 不 受 任何 干扰 ; 其 次 ,与 TCP 相 比 ,UDP 更 加 简单 .高效 ,因此 不 需要 在 两 个 终端 之 
间 建 立 连接 ; 最 后 ,我们 不 必 担 心 数据 包 丢 失 的 问题 ,因为 只 在 一 台 计 算 机 上 使 用 本 地 网 络 通 
信 。 要 建立 此 通道 ,首先 在 MATLAB 和 QTCreator 中 绑 人 
定 UDP 的 特定 端口 。 例 如 ,MATLAB 以 “6667” 结 合 ， 
QTCreator 以 “6666” 结 合 。 然 后 ,如 果 MATLAB 想 要 将 
分 类 结果 发 送 到 QTCreator, 它 可 以 使 用 MATLAB 中 的 
前 数 “frwite (UDP, data)” 将 数据 发 送 到 “6666” 端 口 的 
UDP。QTCreator 将 实时 监控 端口 ,一 旦 MATLAB 发 送 
结果 ,QTCreator 将 立即 收 到 。 如 图 10. 51 所 示 ,训练 结 
后 ,用户 只 需 坐 在 显示 器 前 , 戴 上 电极 帽 ,观看 游戏 场景 即 
本 节 首 先 使 用 运动 想象 设计 BCI 接口 ,接着 使 用 CSP 和 LDA 算法 进行 特征 提取 和 分 类 ， 
并 通过 实验 验证 算法 的 有 效 性 。 然 后 使 用 QTCreator 设计 多 媒体 游戏 。 最 后 ,通过 使 用 TCP 
和 UDP 套 接 字 将 游戏 与 生物 反馈 技术 相 结 合 。 


10.5 本 章 小 结 


本 章 主要 介绍 了 不 同 场景 下 人 机 交互 技术 的 研究 成 果 和 应 用 实例 。 人 机 交互 技术 集成 了 
很 多 学 科 领 域 , 包 括 机 器 人 控制 .机 器 人 感知 .计算 机 视觉 等 。 人 机 交互 一 般 离 不 开 感 知 ( 如 视 
觉 .触觉 .听觉 等 ), 因 此 本 章 介绍 的 几 个 跟 人 机 交互 相关 的 具体 应 用 都 在 不 同 程度 上 使 用 了 传 
感 器 (如 采集 脑 电 图 数据 的 神经 耳机 、 采 集 肌 电信 号 的 MYO 手 环 以 及 采集 视觉 信号 的 Kinect 
设备 ) 。 很 多 章节 都 涉及 这 些 传 感 器 设备 ,包括 第 9 章 , 以 及 第 11 一 13 章 等 。 感 兴趣 的 读者 可 
以 在 本 章 的 基础 上 ,对 人 机 交互 技术 进行 深入 研究 。 


图 10.51 游戏 场景 


机 器 人 还 操 作 技 术 


CHAPTER 11 


机 器 人 和 遥 操作 技术 是 机 器 人 研究 领域 中 的 一 个 重要 分 支 , 它 需 要 多 领域 .多 学 科 相 结合 ， 
例如 控制 理论 、 机 械 工 程 、 人 工 智 能 .电子 、 计 算 机 和 认 知 科学 等 。 机 器 人 到 操作 技术 不 仅 可 以 
作为 航空 航天 (空间 站 的 维护 、 月 球 等 行星 探索 ) ,海洋 探索 .原子 能 应 用 以 及 军事 战争 领域 等 
高 新 技术 领域 发 展 急需 的 关键 性 技术 ,而 且 在 休闲 娱乐 .医疗 等 方面 也 有 重要 作用 。 机 器 人 遥 
操作 技术 可 以 在 复杂 、 和 危险 和 未 知 的 工作 环境 中 来 替代 工人 ,并 能 够 在 人 类 无 法 到 达 的 环境 中 
工作 ,保证 了 作业 的 安全 性 和 高 效 性 。 

遥 操 作 (Teleoperation) 最 早 于 1968 年 由 JohnsenEG 和 CorlissWR 在 技术 报告 中 提出 。 
随 着 科学 技术 的 发 展 ,这 一 概念 发 展 出 多 种 不 同 的 定义 。 有 人 认为 区 操作 技术 是 操作 者 可 以 
远程 控制 具有 感知 能 力 的 机 械 臂 或 者 其 他 设备 运动 的 技术 。 也 有 人 把 遥 操 作 技术 定义 为 远 距 
离 的 操作 , 即 操作 者 的 行为 在 远 距 离 作 用 下 ,仍然 能 够 使 事物 产生 运动 变化 。 

独立 自主 工作 的 机 器 人 系统 与 机 器 人 遥 操 作 系统 的 最 大 区 别 在 于 ,在 独立 自主 工作 的 机 
器 人 系统 中 ,人 只 发 挥 维护 工作 ,实现 机 器 人 的 开始 或 者 停止 工作 ; 而 遥 操 作 系 统 需要 人 机 结 
合 ,操作 者 发 挥 了 更 大 的 自主 性 和 决策 能 力 。 在 遥 操 作 系 统 中 ,将 人 类 的 智慧 和 创造 力 传递 给 
机 器 人 ,使 远程 的 机 器 人 能 够 在 未 知 环境 中 完成 复杂 的 任务 ,弥补 了 独立 自主 机 器 人 系统 的 柔 
韧性 差 .对 未 知 干扰 抵抗 能 力 弱 的 缺点 。 

最 早 的 机 器 人 遥 操 作 系 统 是 主 从 式 机 器 人 系统 ,主要 包括 主机 器 人 、 远 程 操作 者 、 从 机 器 
人 .通信 环节 和 机 器 人 作业 环境 等 。 主 从 式 机 器 人 遥 操 作 的 工作 模式 是 : 通过 利用 计算 机 网 
络 无线电 波 等 传输 媒介 ,将 主机 器 人 的 运动 信号 发 送 给 在 特定 环境 下 作业 的 远 端 从 机 器 人 ， 
同时 , 远 端 从 机 器 人 将 作业 状态 传送 给 近 端 的 操作 者 ,使 操作 者 能 够 根据 远 端 从 机 器 人 的 不 同 
信和 号 做 出 正确 的 决策 。 在 20 世纪 40 年 代 ,Goertz 研发 出 世界 上 第 一 个 主 从 式 遥 操作 系统 。- 
该 遥 操 作 系 统 的 两 个 机 械 臂 采用 主 从 工作 方式 ,操作 者 和 主机 械 臂 在 远离 工作 环境 的 安全 地 
带 , 把 从 机 械 臂 放 在 核 放 射 强 的 工作 环境 中 作业 。 在 安全 环境 的 操作 者 对 主机 械 臂 进行 操作 ， 
而 在 危险 环境 中 的 从 机 械 臂 跟随 主机 械 臂 运动 ,以 完成 作业 要 求 。 由 于 主 从 机 械 臂 之 间 是 通 
过 机 械 装置 进行 通信 的 ,传输 距离 有 限 ,距离 太 长 会 严重 耗费 器 材 ; 并 且 , 从 机 械 臂 对 主机 械 
辟 的 跟踪 效果 较 差 , 所 以 该 主 从 式 遥 操作 系统 具有 很 大 的 局 限 性 。1954 年 ,Goertz 研发 出 世 
界 上 首 个 具有 伺服 反馈 功能 的 机 电 遥 操作 系统 ,该 系统 实现 了 操作 者 对 车 辆 的 遥 操 作 控 制 , 极 
大 地 改善 了 操作 性 能 。 为 减轻 操作 者 对 机 械 臂 的 操作 负担 ,从 机 械 臂 运动 学 和 动力 学 的 角度 
引入 了 双边 控制 法 ,使 得 操纵 杆 更 加 轻便 、 简 单 .易于 使 用 。 例 如 ,一 种 具有 力 反馈 作用 的 遥 操 
作 杆 TouchX ,是 一 种 具有 六 自由 度 的 机 器 臂 模 型 ,操作 者 通过 TouchX 就 可 以 遥 操 作 远 程 机 
器 臂 。 到 了 20 世纪 80 年 代 , 伴 随 着 计算 机 技术 的 快速 发 展 , 人 们 在 机 器 人 遥 操 作 系统 中 逐渐 
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使 用 计算 机 ,从 而 保证 了 先进 控制 算法 的 实现 。 计 算 机 技术 的 发 展 使 机 器 人 遥 操 作 技 术 的 性 
能 发 生 质 的 飞跃 ,也 极 大 地 扩展 了 机 器 人 遥 操 作 技术 的 应 用 领域 。 

本 节 将 介绍 机 器 人 控制 技术 在 遥 操 作 方 向 的 一 些 具体 应 用 ,这 些 应 用 来 源 于 比较 先进 的 
机 器 人 研究 课题 ,是 机 器 人 控制 技术 在 遥 操 作 方 向 较为 典型 的 一 些 应 用 。 


11.1 基于 视觉 融合 技术 的 机 械 璧 三维 遥 操 作 系 统 


残疾 人 由 于 运动 能 力 丧 失 或 感知 能 力 降低 ,他们 很 难 操作 机 器 人 ,无 法 享受 机 器 人 快速 发 
展 带 来 的 种 种 便利 。 为 了 解决 这 个 问题 ,可 以 将 脑 机 接口 技术 与 机 器 人 控制 系统 相互 结合 。 

脑 机 接口 (Brain-Computer Interface，BCI) 技 术 提 供 了 大 脑 和 外 围 设 备 之 间 最 直接 的 通 
路 。 脑 机 接口 能 够 采集 用 户 的 脑 电信 和 号 ,并 对 其 进行 预 处 理 .特征 提取 ,特征 分 类 等 操作 ,最终 
识别 出 用 户 的 意图 。 

然而 ,在 这 些 研究 中 ,我 们 发 现 , 现 有 的 大 多 数 BCI 机 器 人 系统 存在 两 方面 的 问题 。 首 
先 , 在 大 多 数 基 于 SSVEP 的 BCI 系统 中 ,交互 接口 不 友 
好 。 通 常 将 机 器 人 的 视觉 视频 和 刺激 性 画面 分 别 放 在 屏 
幕 上 的 不 同位 置 。 因 此 ,用 户 必 须 注意 机 器 人 视觉 视频 
和 不 同位 置 的 刺激 画面 ,这 降低 了 控制 机 器 人 的 效率 。 
如 图 11.1 所 示 , 为 了 控制 机 器 人 往 不 同方 向 运动 ,用 户 
的 视线 必须 在 屏幕 的 不 同位 置 来 回 切换 (例如 画面 的 正 
上 、 正 下 、 正 左 、 正 右 )。 其 次 ,机 器 人 的 工作 空间 通常 限 
制 为 二 维 平面 。 为 了 将 机 器 人 移动 到 某 个 目的 地 ,操作 
员 必 须 经 常 在 轨迹 中 改变 机 器 人 的 移动 方向 ,这 会 浪费 
用 户 大 量 的 精力 ,造成 不 恨 的 用 户 体验 。 

为 了 解决 第 一 个 问题 ,这 里 使 用 一 种 视觉 方案 ,其 中 
机 器 人 视觉 和 刺激 画面 的 视频 融合 在 一 起 。 不 同 的 刺激 
画面 在 机 器 人 的 视觉 视频 中 传播 了 不 同 的 目标 物体 。 受 试 者 只 需要 专注 于 机 器 人 的 视觉 视 
频 ,而 无 须 适应 视觉 视频 以 外 的 刺激 画面 。 为 解决 第 二 个 问题 ,这 里 采用 共享 控制 策略 , 即 操 
作 员 指定 任务 ,由 机 器 自动 完成 。 本 节 将 通过 脑 电 信号 来 检查 操作 员 是 否 只 需要 在 监视 器 上 
指定 目标 对 象 。 机 器 人 控制 系统 自动 提取 三 维 工作 空间 的 对 象 。 除 了 共享 控制 策略 之 外 , 视 
觉 伺服 (Visual Servo,VS) 技 术 还 能 够 显著 提高 机 器 人 控制 系统 的 准确 性 和 智能 性 。VS 技术 
使 用 视觉 传感器 从 机 器 人 及 其 工作 环境 中 收集 视觉 信息 ,然后 将 其 反馈 给 控制 系统 。 根 据 视 
觉 传感器 的 位 置 ,VC 扫描 的 信息 类 型 分 为 两 种 : 眼 对 手 VS 和 手眼 型 VS。 在 手眼 型 VS 中 ， 
视觉 传感器 连接 机 器 人 末端 执行 器 。 在 眼 对 手 VS 中 ,传感器 安装 在 固定 位 置 。 由 于 手眼 型 
VS 传感器 的 移动 可 能 会 造成 无 法 检测 目标 、 图 像 失真 等 问题 ,这 里 选择 眼 对 手 VS, 并 使 用 
Bumblebee2 立体 相机 获取 工作 环境 中 的 三 维 信息 。 

Tae 景 

1. 视觉 筒 服 系统 分 类 

视觉 伺服 系统 依据 视觉 伺服 的 控制 策略 来 划分 ,视觉 伺服 系统 可 以 分 为 基于 位 置 的 视觉 
伺服 系统 (PBVS) 、 基 于 图 像 的 视觉 伺服 (IBVS) 和 混合 视觉 伺服 三 类 ,它们 的 系统 结构 如 
图 班 :2 地 图 了 :有 所 未 


图 11.1 现 有 研究 中 SSVEP 刺激 
画面 展示 图 
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给 定位 


图 11.2 基于 位 置 的 视觉 伺服 系统 结构 图 


视觉 传感器 


当下 图 像 


给 定 图 像 特征 + 


图 11.3 基于 图 像 的 视觉 伺服 系统 结构 图 


给 定位 姿 + 


Co 2 
A 控制 器 
给 定 旋 转角 度 


当下 位 姿 
图 11.4 混合 视觉 伺服 系统 结构 


2. 基于 脑 机 接口 的 组 成 

脑 机 接口 一 般 由 脑 电信 号 采集 设备 以 及 脑 电 信号 分 析 设 备 两 部 分 构成 ,如 图 11. 5 所 示 。 
脑 电 信号 采集 设备 通常 由 脑 电信 号 放大 仪 及 其 配套 的 脑 电 帽 等 附件 构成 ,主要 用 于 从 用 户头 
部 采集 用 户 的 脑 电 信号 。 信 号 分 析 设 备 负 责 对 脑 电 信号 进行 预 处 理 \ 特 征 提取 ,特征 分 类 等 操 
作 , 从 而 推断 出 用 户 的 脑 电信 号 意图 ,生成 决策 信和 号。 


脑 机 接口 外 部 设备 
图 11.5 脑 机 接口 的 组 成 结构 


11.1.2 脑 机 接口 设计 


本 节 将 具体 介绍 脑 机 接口 的 信号 处 理 算法 ,主要 讨论 两 种 常见 的 脑 机 接口 范式 ,分 别 是 稳 
态 视觉 诱发 电位 (Steady-State Visually Evoked Potentials，SSVEP) 范 式 和 运动 想象 范式 。 
其 中 ,SSVEP 范式 依赖 于 外 界 的 视觉 刺激 诱发 特定 的 脑 电信 号 ,属于 基于 诱发 电位 的 脑 机 接 
口 。 而 运动 想象 范式 则 是 通过 用 户 进行 肢体 运动 的 想象 活动 产生 特定 的 脑 电信 号 ,属于 基于 
自发 电位 的 脑 机 接口 。 

1. 基于 SSVEP 范式 的 脑 机 接口 范式 

人 脑 中 的 神经 细胞 不 断 相互 作用 ,同时 头皮 上 的 电势 也 不 断 变化 ,通过 对 这 些 时 变 势 的 记 
录 和 分 析 , 就 可 以 推断 出 意图 ,这 是 BCI 的 基本 原理 。 例 如 ,在 SSVEP BCI 范式 中 (如 图 11. 6 
所 示 ), 当 用 户 盯 着 一 个 12Hz 的 脑 电 信号 时 ,人 类 EEG 信号 的 频率 特征 将 会 达到 12Hz。 同 
样 地 ,在 15Hz 时 ,人 类 EEG 信号 的 频率 为 15Hz。 基 于 这 种 现象 ,我 们 附加 了 不 同 的 脑 电 信 
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号 ,对 不 同 的 物体 使 用 不 同 的 频率 (如 10Hz、12Hz 和 15Hz) ,并 在 屏幕 上 显示 融合 的 视频 。 当 
用 户 盯 着 目标 物体 时 ,收集 脑 电 图 信号 ,并 对 它们 进行 分 析 以 识别 频率 特征 ,由 此 可 以 确定 用 
户 正 在 盯 着 哪个 物体 。 经 过 该 过 程 ,可 以 通过 SSVEP BCI 推断 用 户 选 择 的 对 象 。 

除了 SSVEP 外 ,还 有 其 他 范式 ,如 运动 图 像 和 P300。 然 而 ,SSVEP 具有 最 高 的 响应 速 
度 ,因此 , 它 是 最 适合 本 节 描 述 的 机 器 人 系统 的 范式 。 这 里 使 用 具有 40 个 通道 的 神经 扫描 放 
大 器 装置 Neuroscan。 它 是 一 种 多 通道 设备 ,具有 高 分 辨 率 和 高 采集 速度 。 选 择 Neuroscan 
装置 上 的 通道 0 .0O,、O.、P;、P， 和 PP, ,它们 适合 产生 SSVEP。 当 传感器 都 工作 在 1000Hz 
的 频率 时 ,记录 用 户 的 EEG 信号 。 


闪烁 模块 : 频率 12Hz 
3X105 


SN dB 
2x105 


EA oo ob 
EEG oe he le Naw 


5 10 15 20Hz 
图 11.6 SSVEP 脑 机 接口 范式 的 基本 原理 


2. 脑 电 信号 处 理 算法 

脑 机 接口 信号 的 处 理 流程 框架 如 图 11.7 所 示 。 首 先 , 利 用 快速 傅 里 叶 变 换 (Fast Fourier 
Fransform,FFT) 对 脑 电 信号 进行 处 理 , 提 取 其 频率 特征 。 由 于 FFT 算法 是 信号 处 理 领 域 的 
一 种 常见 算法 ,这 里 不 做 详细 阐述 。 


i ne 二 省， 


但 


特征 提取 过 程 


i 


图 11.7 SSVEP 范式 脑 机 接口 信号 处 理 流 程 框架 


提取 脑 电 图 信号 后 ,采用 典型 相关 分 析 算 法 (Canonical Correlation Analysis, CCA) 对 其 
特征 进行 分 类 。CCA 对 脑 电 信号 分 类 的 主要 步骤 如 下 。 
假设 存在 两 组 列 变量 S, 一 [Xi,X,,…,X,] 和 S, = 二 [Yi,Y,,*…,Y,],X;€E R”” 和 
YER" ,它们 均 为 列 向 量 。 首 先 对 两 个 变量 进行 如 下 线性 变换 : 
Sw = (WXi rw R22, WmXM) (ll 13 
Sywy=(wiaYi wo WN) (C11.2> 
式 中 ,tw 和, 为 线性 转换 和 矩 阵 。 
利用 CCA 使 S$, 和 S, 之 间 的 相关 系数 达到 最 大 变换 矩阵 , 即 式 (11. 3) 的 结果 达到 最 大 : 
We 
sw | Syw, | 


EEG 


lL,.3) 


pu,v = maxcorr(SiWw 9ywy) = max 
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式 (11. 3) 的 最 大 解 如 下 : 


w -CeCe (11. 4) 
y x 。 
CCC nr =A Os (11. 5) 
其 中 ,C,, 和 C,, 是 协 方差 矩阵 ,C,, 可 以 分 解 为 两 个 矩阵 ,如 下 : 
0,, =R.2Rs; (11. 6) 
定义 
us 一 及 ,ws CTL 7 
则 式 (11.5) 可 以 表示 为 : 
RG CO RD (11. 8) 
进而 可 得 
A =R LC, COERE! (11. 9) 


Tr I 


则 式 (11.9) 变 为 Au, = 二 Xu,。 因 此 ,wu, 可 以 计算 为 矩阵 A 的 特征 向 量 , 基 于 式 (11. 4) 和 
式 (11.7) ,可 以 计算 出 w。 和 w, 。 然 后 由 式 (11. 3) 计 算出 两 个 变量 之 间 的 相关 系数 。 

通过 采用 上 述 算法 ,建立 了 EEG 信号 和 参考 信号 之 间 的 相关 性 。 通 过 比较 它们 ,并 取 最 
大 值 , 可 以 知道 用 户 最 喜欢 的 选择 。- 

3. 运动 想象 范式 的 脑 机 接口 的 信号 处 理 算法 

运动 想象 范式 的 脑 机 接口 的 信号 处 理 过 程 如 图 11. 8 所 示 。 采 集 到 脑 电 信号 之 后 , 先 用 共 
同 空 间 模 式 算法 (Common Spatial Pattern，CSP) 对 脑 电 信号 进行 特征 提取 ,然后 利用 支持 向 
量 机 (Support Vector Machine，SVM) 算 法 进行 分 类 ,从 而 推断 出 当前 脑 电信 号 属于 哪 种 
类 别 。 


图 11.8 运动 想象 范式 脑 机 接口 的 信号 处 理 过 程 


1) 运动 想象 脑 机 接口 的 理论 基础 

当 人 在 进行 运动 或 想象 身体 的 运动 时 ,具体 表现 为 脑 部 特定 区 域 的 EEG 信号 的 频率 特征 
上 节律 (8~12Hz) 和 8B 节律 (18~26Hz) 会 发 生 改变 (ERD 现象 ); 反之 ,又 会 重新 恢复 (ERS 现 “ 
象 )。 在 频谱 图 中 ,这 种 能 量变 化 过 程 如 图 11. 9 所 示 。 由 图 分 析 可 知 , 在 分 析 数 据 时 ,需要 先 
将 节律 和 8B 节律 的 信号 从 原始 信号 中 提取 出 来 。 这 里 采用 想象 左右 手 的 运动 ,人 脑 对 左右 
手 运 动 的 感觉 和 控制 区 域 如 图 11. 10 所 示 。 

因此 ,根据 人 运动 想象 时 特定 区 域 的 脑 电 信号 能 量变 化 规律 ,可 以 推断 人 是 否 在 进行 运动 

2) 运动 想象 脑 机 接口 特征 提取 算法 

这 里 采用 CSP 算法 提取 运动 想象 时 的 脑 电 信号 特征 ,该 算法 对 二 分 类 的 脑 机 接口 分 类 正 
确 率 比 较 高 。 使 用 CSP 算法 对 二 分 类 脑 电 信号 分 类 的 计算 过 程 ,请 参考 本 书 10. 4. 1 节 , 这 里 
不 再 重复 。 
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0 3 10 13 20 


频率 
图 11.9 运动 想象 信号 的 频 域 分 布 特征 : 图 11.10 运动 想象 脑 电信 号 生理 特征 
ERS/ERD 现象 的 空间 分 布 


3) 运动 想象 脑 机 接口 的 特征 分 类 算法 

对 脑 电 信号 特征 进行 分 类 的 算法 有 很 多 ,结合 脑 电 信号 特征 ,这 里 选择 支持 向 量 机 
(SVM) 算 法 对 其 进行 分 类 。SVM 算法 的 主要 思想 是 ; 通过 求解 一 个 最 优化 平面 来 分 离 两 类 
样本 ,同时 保证 该 平面 距离 两 类 样本 之 间 的 间距 之 和 为 所 有 可 能 的 平面 中 的 最 大 值 。 

SVM 算法 的 基本 原理 如 图 11. 11 所 示 , 即 通过 寻找 最 优 斜率 w 和 截 距 2 ,使 超 平面 到 样 
本 点 的 最 小 距离 为 所 有 可 能 情况 中 的 最 大 值 。 


(a) 情况 一 (b) 情况 二 
某 截 距 下 的 最 短 距 离 某 截 距 下 的 较 短 距离 


(c) 情况 三 (d) 情况 四 
图 11.11 SVM 算法 的 基本 原理 


对 于 线性 不 可 分 的 情况 ,SVM 算法 的 目标 为 ,在 约束 条 件 式 (11. 10) 的 限制 下 : 
昌江 太 妥 荔 久 用 2] 芝 寺 一 条， 本 2 C11. 10 
找到 使 得 目标 函数 ( 即 式 (11. 11)) 的 值 达 到 最 小 值 的 参数 值 . 


1 
gw) = 这 el) SC S36 (i 
i 一 1 
其 中 ,w 是 分 类 平面 的 斜率 ,2 为 截 距 ,C 二 0 称 为 惩罚 因子 ,6 表示 松弛 因子 。 
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上 述 问 题 实际 上 是 一 个 凸 二 次 规划 问题 , 即 : 


, 1 2 ， 世 
minspe 5 Nol’+C 仿生 
zi 一 1 
以 满足 
yitw XT 1 T=12rN 
CL E22) 


S20, i=1,2,°"*sN 
求解 该 问题 的 最 常用 方法 就 是 利用 序列 最 小 最 优化 算法 (Sequential Minimal Optimization， 
SMO) 进 行 最 优化 寻 解 (具体 过 程 可 参考 相关 资料 ,此 处 不 再 袭 述 )。 

经 过 以 上 步骤 ,可 得 到 如 下 分 类 函数 : 


l 
ysign| DearyiK Cis) + C1 1 
1 一 ] 


11.1.3 ”机械 句 三 维 吵 操作 系统 控制 部 分 设计 

本 节 描 述 了 具有 智能 接口 的 机 器 人 控制 系统 的 开发 。 当 我 们 将 目标 对 象 的 坐标 发 送 给 机 
器 人 控制 器 时 ,机 器 人 控制 器 会 自动 到 达 目 标 位 置 并 抓 取 目 标 。 同 时 ,控制 器 将 检测 机 械 臂 周 
围 的 所 有 障碍 物 ,并 规划 合适 的 轨迹 ,避免 与 环境 中 的 障碍 物 发 生 碰 撞 。 

1. Baxter 机 器 人 运动 学 与 避 障 

Baxter 机 器 人 如 图 11. 12 所 示 。 在 每 个 臂 的 末端 有 一 个 可 编程 的 “夹子 ”, 可 以 用 作 抓 取 
物体 的 末端 执行 器 。 根 据 Baxter 机 器 人 的 运动 学 模型 , 笛 卡 儿 空 间 xz 中 的 末端 执行 器 的 坐标 
和 Baxter 机 械 臂 的 关节 角度 dg 满足 以 下 等 式 : 

= (11. 14) 

其 中 ,x ER",g€ER',f 是 从 关节 角度 空间 到 笠 卡 儿 空 间 的 非 线性 映射 。 


(a) Baxter 机 器 人 (b) 七 自由 度 机 器 人 的 机 械 辟 
图 11.12 Baxter 机 器 人 外 观 


由 于 x 和 g 之 间 的 非 线 性 关系 ,使 得 计算 复杂 且 耗 时 。 为 避免 这 个 问题 ,对 式 (11. 14) 求 

导 , 可 得 线性 方程 如 下 : 
天 二 (11.15) 

其 中 , 守 ER'* 是 笛 卡 儿 空 间 中 末端 执行 器 的 速度 ,g ER' 是 角速度 矢量 ,由 机 械 臂 中 每 个 关节 
的 所 有 角速度 值 组 成 。JER'" 是 雅 可 比 矩 阵 。J 随 末 端 执行 器 的 位 置 而 变化 , 即 x 的 值 。 

由 于 速度 之 的 维度 6 小 于 关节 角速度 4d 的 维度 7, 所 以 式 (11.15) 有 无 穷 多 个 解 , 即 机 械 
臂 可 以 通过 不 同 的 姿态 到 达 同 一 目标 点 ,如 图 11. 13 所 示 。 因 此 ,根据 线性 代数 知识 , 式 (11. 15) 
( 即 0 ) 的 解 可 以 拆 分 为 如 下 两 部 分 ， 


第 11 章 “机 器 人 遥 操 作 技术 |‖l 萝 287 


2 


4 一 和 ,十 0， st CHIL6) 
其 中 ,0, 是 式 (11. 15) 的 特 解 ,9, 为 其 通 解 。 从 物理 意义 上 解释 式 (11. 16) ,9, 是 速度 分 量 , 它 
确保 了 机 械 臂 的 末端 执行 器 到 达 目 的 地 。0， 
的 速度 分 量 在 雅 可 比 和 矩阵 JJ 的 零 空 间 中 , 它 的 
值 仅 改变 机 械 臂 的 姿势 ,不 影响 末端 执行 器 的 
位 置 Cz 的 值 ) 。 因 此 ,可 以 通过 调整 5, 的 值 
确保 机 械 臂 在 不 影响 9, 任务 性 能 的 情况 下 而 
过 度 运 动 。 这 是 元 余 机 械 臂 避 障 的 基础 ,如 
图 11. 13 所 示 。 

2 为 使 Baxter 机 器 人 能 够 避 开 障碍 物 , 这 里 图 11.13 完 余 机 械 臂 避 障 的 基本 原理 : 
采用 CTS 方法 ,具体 如 下 。 通过 不 同 的 姿态 实现 避 障 

首先 ,定义 一 个 指标 五 用 于 估计 障碍 物 和 

机 械 臂 之 间 的 距离 ,如 下 : 
EY = min(dy yy ,d= 1 R37 Ci 17) 

其 中 ,qd; 是 障碍 物 与 机 械 臂 的 关节 i 之 间 的 距离 。 互 是 &， 和 距离 指示 器 之 间 的 最 小 距离 。 
如 果 互 的 值 小 于 阔 值 , 则 有 可 能 发 生 冲 突 ,应 采取 相应 措施 避免 这 一 问题 。 

对 式 (11. 17) 求 导 , 可 得 : 


沙河 殉 识 苦 


dH 9H dg 
dt 9g dt 

因为 互 的 值 表 示 障 碍 物 和 机 械 臂 之 间 的 距离 ,所 以 互 的 微分 ( 即 卫 的 值 ) 可 用 于 估计 机 
械 臂 的 运动 结果 。 当 本 小 于 0 时 , 玉 的 值 减 小 ,这 样机 械 臂 会 向 靠近 障碍 物 的 方向 移动 。 当 
0 时 ,机 械 臂 则 向 远离 障碍 物 的 方向 移动 。 根 据 式 (11. 16), 可 以 将 本 分 为 和 厂 , 两 部 
分 ,如 下 : 


TI 


=VH .4 (11. 18) 


=VH 9, 
Tr,=VH .0， l (11. 19) 
T=VH .=VH .9,+VH .9,=T,+T, 

其 中 ,T, 和 本 ,用 于 估计 了, 和 ,的 影响 ,类 似 于 荆 。 如 果 工 ,二 0, 则 机 械 臂 的 轨迹 是 安全 的 ， 
并 且 机 械 臂 正 远离 障碍 物 。 如 果 工 ,二 0, 是 互 小 于 闪 值 , 则 机 械 臂 靠近 障碍 物 并 朝向 它 移动 ， 
很 可 能 会 发 生 碰 撞 。 在 这 种 情况 下 ,应 该 合理 设置 5, 的 值 以 使 机 械 臂 避 障 。 

2. 任务 运动 和 自 运动 的 生成 

为 了 生成 任务 运动 , 式 (11. 16) 可 以 表示 为 

4 一 Giz 二 (ciJ 一 T)Z=Gi 之 十 RCI — GJ)VH (Il 20) 

其 中 ,G; 和 Gs 是 JJ 的 广义 闭 矩 阵 , 即 JG1J = 二 J 和 JG,J = 二 J。I, 是 维 数 为 n Xn 的 单位 矩 
阵 ,ZE7X1 是 任意 向 量 , 可 以 设 为 Z= 二 一 & VH ,其 中 是 实数 标量 ,VH 是 式 (11.17) 中 五 
的 梯度 。 通 过 式 (11. 20), 可 以 生成 &, 和 4, 的 值 。 此 外 ,通过 改变 Gs 中 的 参数 ,可 以 调整 
式 (11.19) 中 卫 ,d 的 值 大 于 0 或 小 于 0, 从 而 控制 机 械 臂 远离 障碍 物 或 者 靠近 障碍 物 。 

3. 避 障 实验 

为 了 验证 机 器 人 避 障 效果 ,首先 将 Baxter 机 械 臂 抽象 成 几 根 相连 的 直线 ,如 图 11. 14(a) 
所 示 。 令 机 械 臂 从 其 工作 空间 中 的 一 点 移动 到 另 一 点 ,并 记录 每 个 环节 的 位 置 。 没 有 障碍 物 
时 ,Baxter 机 械 臂 的 运动 轨迹 如 图 11. 14(b) 所 示 , 有 障碍 物 时 的 运动 轨迹 如 图 11. 14(c) 所 示 。 
由 图 11. 14(b) c) 可 知 ,使 用 CTS 方法 使 Baxter 机 械 辟 能 够 避 开 轨迹 中 的 障碍 物 而 不 影响 
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其 在 末端 执行 器 中 的 任务 。 


a 
PE 运动 方向 一 一 


0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 
到 


(b) 无 障碍 物 时 机 械 劈 的 运动 轨迹 (c) 有 障碍 物 时 机 械 臂 的 运动 轨迹 
图 11.14 机 械 臂 避 障 验证 试验 轨迹 展示 图 


11.1.4 机 械 臂 三 维 遥 操作 系统 的 视觉 伺服 部 分 设计 


视觉 伺服 系统 的 主要 作用 有 3 种 ,如 图 11. 15 所 示 。Bumblebee2 相机 上 并 排放 置 两 个 相 
机 ,根据 其 位 置 分 别 命名 为 左 眼 相机 和 右 眼 相机 ,相应 地 ,它们 拍摄 的 照片 分 别 命名 为 左 眼 照 
片 和 右 眼 照片 。 可 以 看 出 , 左 眼 相机 和 右 眼 相机 分 别 拍摄 出 来 的 左 眼 照片 和 右 眼 照片 是 有 区 
别 的 。 根 据 这 些 区 别 ,计算 可 得 照片 中 的 物体 的 三 维 坐标 信息 。 

1.. 目标 物体 识别 与 自 适应 颜色 追踪 算法 

VS 部 分 的 第 一 个 任务 是 检测 由 Bumblebee2 相机 捕获 的 图 像 中 的 物体 位 置 ,然后 将 它们 
作为 视觉 反馈 显示 给 操作 员 , 如 图 11. 15(b) 所 示 。 根 据 它 们 的 颜色 将 目标 物体 与 图 像 分 离 。 
为 了 检测 它们 的 位 置信 息 ,目标 物体 的 颜色 应 该 与 背景 颜色 不 同 。 识 别 物体 的 具体 步骤 如 下 。 
首先 ,研究 人 员 绘 制图 像 的 外 部 轮廓 。 然 后 ,计算 出 所 有 像素 点 的 颜色 平均 值 ,平均 值 可 以 作 
为 表示 对 象 的 这 些 特征 的 “标准 颜色 标记 ”。 最 后 ,在 操作 实验 阶段 ,从 实时 图 像 中 计算 出 标准 
颜色 标记 与 图 像 中 每 个 点 的 颜色 信息 之 间 的 差异 。 最 小 方差 值 表示 最 可 能 包含 目标 对 象 的 
区 域 。 

然而 ,在 实验 过 程 中 发 现 物体 在 拍摄 时 的 颜色 发 生 改 变 , 这 是 因为 光 在 空间 中 的 不 均匀 分 
布 。 因 此 ,使 用 单个 颜色 标记 来 表示 对 象 不 会 产生 稳定 的 结果 。 此 外 ,对 象 的 颜色 有 时 变化 很 
大 ,并 且 不 能 在 图 像 中 检测 到 。 为 了 解决 这 个 问题 ,这 里 使 用 一 种 自 适应 颜色 识别 算法 ,该 算 
法 的 描述 如 表 11. 1 所 示 。 
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十 沈 莹 时 


于 渤 疆 证 


0 


(a) Bumblebee2 相 机 拍摄 的 原 图 


(c) 任务 2 : 计算 物体 的 三 维 坐标 (d) 任务 3 : 坐标 系 转换 
图 11.15 ”视觉 伺服 系统 的 3 个 主要 任务 


表 11.1 自 适应 颜色 识别 算法 
描 述 
使 用 上 次 计算 的 标准 颜色 特征 标签 识别 物体 
若 成 功 检测 出 物体 , 则 重新 计算 检测 区 域 中 心 点 附近 区 域 的 颜色 平均 值 

用 步骤 四 中 的 平均 值 替 换 原 标准 颜色 特征 标签 ,返回 步骤 四 进行 下 一 次 检测 


为 了 验证 自 适应 算法 的 有 效 性 ,设计 如 图 11.6(a) 所 示 的 实验 。 设 计 三 段 不 同 的 轨迹 作 
为 纯色 物体 的 运动 轨迹 ,在 每 段 轨迹 中 ,捕获 100 张 图 片 记录 物体 的 运动 。 然 后 ,分 别 使 用 自 
适应 算法 和 非 自 适应 算法 来 跟踪 100 张 图 片 中 的 所 有 对 象 ,计算 成 功 跟 踪 的 图 像 数 量 , 实 验 结 
果 如 图 11. 16(b) 所 示 。 


| 。” 轨迹 诽 自 适应 算法 | 自 适应 算法 


一 86/100 100/100 
100 张 图 片 67/100 100/100 


94/100 100/100 


(b) 实验 结果 : 成 功 追 踪 到 的 图 片 数量 
图 11.16 自 适应 跟踪 算法 实验 示意 图 及 结果 


2. 双 目 视觉 和 深度 测量 
同一 物体 的 位 置 在 左 眼 相 机 和 右 眼 相机 中 的 显示 结果 是 不 同 的 ,如 图 11. 15(a) 所 示 。 同 
一 物体 在 左右 眼 相 机 中 的 差异 计算 如 下 : 
(ne El 
其 中 ,zier 和 zaght 分 别 是 左 眼 图 像 和 右 眼 图 像 中 相同 对 象 的 z 坐标 。 
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根据 差异 4 ,对 象 的 深度 信息 计算 如 下 : 
一 
2, = 二 22) 
其 中 ,2Z. 是 Bumblebee2 相机 坐标 系 中 的 Z 坐标 ,也 是 物体 与 Bumblebee2 相机 的 深度 值 ; f 
是 相机 的 焦距 ,L 是 Bumblebee2 的 两 个 传感器 之 间 的 距离 ; 入 和 上 的 值 是 一 个 常数 ,可 以 从 
Bumblebee2 相机 的 参数 中 读 取 。 


然后 ,使 用 以 下 等 式 计 算 Bumblebee2 相机 坐标 系 中 对 象 的 X 和 YY 坐标 : 


l 2 (eR Ae (00) 
工 。 一 
7 Cl 2 
| 之 c (Yleft 广 * 0.5) 
a i 
其 中 ,zx。 和 y。 分 别 是 摄像 机 坐标 系 中 对 象 的 X 和 六 坐标 ,常数 Xi。 。 和 Yi。 分 别 是 图 像 


的 高 度 和 高 度 。 

3. 改进 的 手眼 标定 方法 

现在 已 经 得 到 Bumblebee2 摄像 机 坐标 系 中 物体 的 三 维 坐标 。 但 是 ,在 Baxter 机 器 人 控 
制 系统 中 ,这 些 坐标 必须 根据 Baxter 坐标 系 来 描述 。 因 此 ,这 里 将 对 象 的 坐标 从 Bumblebee2 
坐标 系 转换 为 Baxter 机 器 人 坐标 系 。 转 换 方程 如 下 : 


2 二 
yc Yr 

Tx 一 (11. 24) 
及 2 之 
1 1 


其 中 ,TER*” 是 变换 矩阵 ,T.; 是 T 矩阵 的 第 i 列 ; [z。 y。 xz。 1]' 是 Bumblebee2 坐标 


系 中 的 坐标 ,[x， y， xz， 1] 是 Baxter 机 器 人 坐标 系 中 的 坐标 。 
tl itis tia th 
a ee (1.2 
tsl zt32 zt33 ta4 
0 0 0 ] 
基于 式 (11. 24) ,可 以 用 式 (11. 26) 实 现 变换 矩阵 T。 
we dy re wg), [al en mass oul 
| ya Wo Vlad 
Trl Tr2 Tr3 Zl Xe Yea Zea 
下 1 eke 1 1 由 
其 中 ,[z， y,，z, 1] 和 [zy xz 1j'(i==1,2,3,4) 是 在 Baxter 机 器 人 坐标 系 
和 Bumblebee2 相机 中 测量 的 四 个 非 平面 点 的 坐标 。 

这 里 还 需要 考虑 如 何 测量 式 (11. 26) 中 的 坐标 。 可 以 通过 式 (11. 22) 和 式 (11. 23) 测 量 
Bumblebee2 坐标 系 中 一 个 点 的 坐标 。 要 测量 Baxter 坐标 系 中 的 坐标 ,可 以 使 用 标尺 来 确定 
目标 点 与 坐标 系 原 点 之 间 的 距离 。 但 是 ,Baxter 坐标 系 的 原点 不 可 用 ,因为 它 位 于 Baxter 坐 
标 系 内 。 此 外 ,在 测量 中 很 难保 证 标尺 的 水 平 度 和 垂直 度 。 因 此 ,需要 找到 另 一 种 方法 来 测量 
Baxter 坐标 系 中 的 坐标 。 可 以 使 用 Baxter 的 运动 学 API, 用 它 来 读 取 机 器 人 未 端 执行 器 的 坐 
标 ,并 命令 末端 执行 器 到 达 所 有 坐标 的 位 置 。 因 此 ,在 校准 实验 中 ,通过 式 (11. 26) 使 用 


Yi Jr2 Yr3 yr4 (11. 26) 
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Bumblebee2 相机 检测 并 记录 末端 执行 器 的 坐标 为 [r。 y。 > (i 二 1,2,3,4)。 使 用 机 器 人 
API 在 式 (11. 26) 中 读 取 并 记录 Baxter 机 器 人 坐标 系 中 末端 执行 器 的 坐标 为 [zx,， y,， zz 以 
计算 变换 矩阵 T。 
但 是 ,由 于 机 器 人 API 和 Bumblebee2 相机 中 测量 的 不 精确 性 ,随机 误差 始终 存在 ,这 些 
问题 会 影响 校准 效果 。 为 减少 误差 ;考虑 引入 LSM 进行 校准 。LSM 表达 式 如 下 : 
AxP=B Cl 2 
其 中 ,A ER”*” 和 BER”” 是 具有 常数 值 的 矩阵 。PER"”: 表示 要 求解 的 未 知 参数 。 通 
常 ,m 和 nn 之 间 的 关系 是 如 过 2。 因此 , 式 (11. 27) 没 有 解 , 也 就 是 说 ,无 法 找到 也 的 向 量 满足 
式 (11.27)。 为 了 解决 这 个 问题 ,这 里 使 用 LSM 来 寻找 PP 的 解 ,根据 LSM 的 理论 ,P 向 量 计 
- 算 如 下 : 
卫 一 (4TxA4A) '*xA'xB 加 工区) 
虽然 式 (11. 27) 与 式 (11. 24) 的 形式 类 似 , 但 它们 的 实际 差别 很 大 。 式 (11. 27) 中 要 求解 的 
参数 在 P 中 ,而 在 式 (11. 24) 中 ,矩阵 T 是 需要 求解 的 。 因 此 ,LSM 算法 不 能 直接 用 于 求解 变 
换 和 矩阵 T 的 值 。 这 里 采用 一 种 改进 LSM 算法 进行 校准 。 经 过 一 系列 推导 , 式 (11. 24) 与 表达 
式 (11. 29) 等 价 。 
| 


Yrl 
之 1 
EI Wl worly Ye ] 
wle. Nels Selly yl IT =27 (11. 29) 
: : : di l | 
Tm 
xenla | zonls 1 Teg 
Yr 
之 mm 
|1 


其 中 ,四 ER 是 单位 矩阵 。T。 是 变换 矩阵 T 中 的 第 ; 列 向 量 , 如 式 (11. 25) 所 示 。 
[zs yc xz 和 [zy zx] 是 在 Bumblebee2 坐标 系 和 Baxter 坐标 系 中 测量 的 一 系列 
点 的 坐标 。 将 式 (11. 29) 简 化 成 如 下 形式 : 

KLsm * TisM —=X,Lsm (11. 30) 

在 该 变换 中 , 式 (11. 30) 中 的 第 二 个 元 素 (T, sw ) 是 要 求解 的 16X1 的 参数 向 量 。 然 后 
式 (11. 28) 可 用 于 计算 T sw 的 最 小 二 乘 误 差 结果 : 

TLsm =《 关 二 SM 7 > 关 X,Lsm C11. 31) 

在 计算 完 矢量 Tjsm 的 值 之 后 ,根据 式 (11. 43) 将 其 变换 为 变换 矩阵 T 的 形式 。 通 过 以 上 
过 程 ,LSM 就 可 以 用 来 解决 校准 问题 。 

为 了 验证 式 (11. 31) 的 校准 效果 ,设计 以 下 实验 。 第 一 步 ,分 别 使 用 有 LSM 和 无 LSM 的 
算法 并 进行 比较 ,以 获得 两 个 变换 矩阵 T 和 T,。 第 二 步 , 令 Baxter 的 末端 执行 器 在 其 工作 
空间 中 达到 10 个 不 同 的 点 。 第 三 步 ,通过 Bumblebee2 相机 记录 10 个 点 在 摄像 头 坐 标 系 中 的 
坐标 值 , 记 为 CuwE3X1, 并 使 用 机 器 人 APT 读 取 这 10 个 点 在 机 器 人 坐标 系 中 的 坐标 值 , 记 为 
CapE3X1。 显 然 ,Cuun 和 Ci 是 测量 得 到 的 精确 坐标 值 。 第 四 步 , 通 过 T。T。 和 式 (11. 24)， 
将 Ctun 转换 为 Baxter 机 器 人 坐标 系 下 的 坐标 , 记 为 CuE3X1。 显 然 ,Css 并 不 是 测量 得 到 
的 真实 坐标 ,因此 其 值 可 能 存在 误差 。 第 五 步 , 通 过 比较 Cl 和 C. 中 的 误差 (Cerrors 王 
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Ci 一 Cl) ,验证 LSM 算法 的 效果 。 误 差 计算 如 下 : 
errory; = abs(Ty * Ca 一 Con) 
C11..32) 
srtot, 后 abs(T RC = Cp 


其 中 ,error,, 表示 使 用 LMS 算法 的 误差 ,error, 表示 不 使 用 LSM 算法 的 误差 ,“abs()” 表 示 绝 
对 值 的 计算 。 3 

Cwob 和 Ceu 的 位 置 如 图 11. 17 所 示 , 每 个 点 的 误差 值 如 图 11. 18 所 示 。 由 图 11. 18 可 得 ， 
使 用 LSM 算法 和 不 使 用 该 算法 的 平均 误差 分 别 为 0.035m 和 0. 010m ,说 明 使 用 LSM 算法 的 
实验 结果 较 好 。 


LSM 前 


一 一 一 一 于 一 一 


0.4[ 。 实际 点 
Xx 计算 得 到 的 点 J -二 -上 -一 - ~ 
1 1 1 9 


/ 
en 


st wi i wi i til ni) ein ea 


(a) 在 不 使 用 LSM 算 法 的 情况 下 比较 实际 点 和 计算 点 。”(b) 使 用 LSM 算 法 时 比较 实际 点 和 计算 点 
图 11.17 LSM 实验 的 结果 


错误 率 比较 


国 是 LSM 前 
国 到 LSM 后 


图 11.18 两 种 方法 误差 值 对 比 图 


11.2 基于 肌 电 信号 的 移动 机 器 人 到 操作 控制 


本 节 将 介绍 一 种 基于 操作 者 手臂 表面 肌 电信 号 的 变 增益 控制 器 。 

运动 规划 是 移动 机 器 人 工作 的 一 项 重要 任务 一 一 寻找 目标 的 期 望 路 径 、 同 时 躲避 障碍 物 。 
势 场 法 (Potential Field Method,PFM) 是 一 种 常用 的 避 障 方案 ,其 逻辑 简单 ,数学 表达 式 清晰 。 
PFM 将 机 器 人 的 工作 空间 视 为 充满 人 工 势 场 的 空间 ,在 势 场 中 工作 的 移动 机 器 人 受到 两 种 势 
力 的 作用 , 即 目标 点 产生 的 引力 和 障碍 物产 生 的 斥 力 。 通 过 引力 与 斥 力 的 结合 ,可 以 计算 出 总 
力 的 方向 和 大 小 ,移动 机 器 人 将 会 到 达 目 标 ,并 沿 着 这 个 方向 避 开 障碍 物 。 然 而 ,传统 的 势 场 
法 存在 一 些 固 有 的 局 限 性 ,局 部 极 小 值 是 导致 移动 机 器 人 无 法 避 开 障碍 物 或 达到 目标 位 置 的 
最 重要 的 局 限 性 之 一 。 考 虑 机 器 人 与 目标 的 相对 距离 ,有 学 者 提出 了 一 种 新 的 排斥 力 势 函数 ， 
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题 , 有 学 者 采用 粒子 群 优化 算法 对 人 工 势 场 方法 进行 了 修改 和 优化 。 目 前 ,PFM 不 仅 可 以 用 
于 基站 ,还 可 以 用 于 动态 环境 。 

针对 移动 机 器 人 的 目标 和 障碍 物 与 移动 方向 一 致 的 问题 ,本 节 将 介绍 一 种 新 的 共享 遥控 
方案 ,使 移动 机 器 人 能 够 避 开 障碍 物 ,* 从 一 个 位 置 移动 到 目标 位 置 。 遥 操作 是 一 种 由 人 工 操作 
对 从 端 机 器 人 进行 远程 精确 控制 的 技术 。 利 用 支持 向 量 机 (SVM) 并 根据 肌 电信 和 号 来 识别 人 
体 手势 ,人 类 操作 者 通过 手势 控制 移动 机 器 人 的 运动 方向 。 为 了 提高 工作 效率 和 安全 性 ,利用 
人 手臂 的 刚度 来 控制 移动 机 器 人 的 速度 。 通 过 PFM 与 操作 者 共同 控制 移动 机 器 人 的 运动 。 
与 PFM 控制 的 移动 机 器 人 相 比 ,移动 机 器 人 可 以 更 快 地 避 开 障碍 物 , 克 服 局 部 极 小 问题 。 为 
“了 验证 共享 控制 方法 的 有 效 性 ,操作 者 利用 深度 摄像 机 遥 操 作 一 个 全 向 移动 机 器 人 
(Omnidirectional Mobile Robot, OMR) 来 避 开 障碍 物 并 达到 目标 位 置 。 深 度 相机 可 以 测量 出 
障碍 物 与 OMR 之 间 的 最 近 距 离 , 当 障碍 物 在 OMR 运动 方向 的 安全 距离 内 时 ,操作 者 可 以 从 
MYO 手 环 获得 反馈 。 


11.2.1 遥 操 作 系 统 结构 

遥 操 作 系统 如 图 11. 19 所 示 。 操 作 人 员 在 每 个 前 臂 上 分 别 佩戴 两 个 MYO 手 环 。 利 用 采 
集 到 的 表面 肌 电信 号 ,利用 MYO 手 环 对 操作 者 的 手势 进行 识别 ,利用 另 一 个 MYO 手 环 对 人 
体 手臂 的 刚度 进行 估计 。MYO 手 环 通过 蓝牙 向 计算 机 发 送 肌 电信 号 ,运动 方向 和 速度 可 以 
通过 人 体 姿态 和 手臂 的 刚度 来 确定 。OMR 配备 了 一 台 客 户 端 电 脑 和 一 台 华硕 Xtion2 摄像 
头 (深度 和 RGB 摄像 头 ,垂直 视角 52", 水 平视 角 74*) 。 采 用 Xtion2 摄像 头 捕捉 OMR 的 运 
动 环境 ,检测 OMR 与 障碍 物 之 间 最 近 的 距离 ,设备 采集 的 深度 范围 为 0.8 一 3. 5m。 客 户 端 
计算 机 接收 运动 指令 ,将 运动 环境 和 OMR 与 障碍 物 之 间 的 最 近 距 离 发 送 给 操作 员 。 当 障 
碍 物 与 机 器 人 之 间 的 距离 小 于 安全 距离 时 ,操作 者 可 以 通过 使 MYO 手 环 振动 来 接收 客户 
端 计算 机 的 反馈 。 在 此 基础 上 ,利用 人 工 势 场 方法 实现 了 对 OMR 的 协同 控制 , 避 开 不 同 的 
障碍 物 。 


前 _ 停 


sEMG 信 号 


i 
手势 和 刚度 和 


图 11. 19 移动 机 器 人 遥 操 作 系统 示意 图 
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11.2.2 ”基于 表面 肌 电 信号 的 人 机 遥 操 作 控 制 

本 节 将 介绍 基于 肌 电 信号 的 手势 控制 ,对 移动 机 器 人 施加 引力 场 ,改变 原 有 的 引力 和 斥 力 
场 关 系 , 并 引入 利用 操作 者 的 手臂 刚度 来 控制 移动 机 器 人 的 速度 。 

1. 基于 肌 电 信号 的 手势 识别 

基于 人 体 手势 识别 的 表面 肌 电 信号 由 离线 训练 和 在 线 识 别 两 部 分 组 成 。 在 线 识 别 是 根据 
加 工 后 的 表面 肌 电 信和 号 的 不 同 特征 进行 分 类 。 离 线 训练 包括 数据 采集 .数据 处 理 、 特 征 提取 和 
降 维 等 步骤 。 这 里 采集 了 15 名 健康 人 群 (18 一 25 岁 ) 前 臂 佩戴 MYO 手 环 时 的 四 种 手势 作为 
训练 样本 (如 图 11. 20 和 图 11. 21 所 示 ) ,每 个 手势 采集 5000 个 样本 。 数 据 处 理 包 括 样本 校 
正 、 取 均 方 和 平滑 滤波 等 。 在 特征 提取 部 分 ,这 里 采用 时 域 特征 ,主要 包括 取 标 准 差 , 平 均值 、 
绝对 值 积 分 、 根 方 均值 .过 零点 数 和 Willison 振幅 等 。MYO 手 环 可 通过 8 个 生物 电 传 感 器 测 
量 前 臂 肌 电 图 的 8 个 通道 ,因此 有 40 个 特征 可 用 于 识别 。 为 了 解决 数据 压缩 过 程 中 的 维 数 灾 
难 问 题 ,减少 数据 损失 ,采用 主 成 分 分 析 法 进行 降 维 。 其 中 ,标准 差 .平均 值 . 绝 对 值 积分 、 根 方 


均值 的 计算 公式 分 别 为 : 
std = 
AV= 
J/ (11. 33) 
RMS= 
N | 
ZC= >)sgn( 一 zizi 1), sn 一 | 
i=1 0 区 -人 罗 
其 中 ,NN 为 每 次 采样 的 数量 ,z; 表示 i 个 样本 的 肌 电 信号 幅 值 ,b 表示 NN 个 样本 的 平均 值 。 
a sSEMG 的 绝对 积分 值 | sSEMG 的 绝对 积分 值 
60 3 多 mot 
50 0 oo 。 50 攻 访 ' Es 
咏 4 ep 篇 和 9 
咖 30 by 人 要 出 30 上 si 
和 村 ” Eo YY AN Cy 
20 i ." he 。 First 20 » First 
~ * Moh « FingersSpread * FingersSpread 
0 , tb Wavein mn Wavein 


OH。 s WaveOnut * WaveOut 
0 10 20 30 40 0 1 3 3 0 5 0 态 
通道 2 通道 3 
图 11.20 第 2 和 第 3 通道 的 绝对 值 积 分 样本 图 11.21 第 3 和 第 6 通道 的 绝对 值 积 分 样本 


2. 势 场 法 

为 了 简单 起 见 , 将 OMR 看 作 是 在 二 维 工作 空间 中 控制 的 点 质量 。OMR 的 运动 方向 由 斥 
力 和 引力 的 合力 决定 ,引力 由 目标 位 置 产生 ,障碍 物 对 移动 机 器 人 产生 斥 力 。 合 力 场 和 合力 计 
算 分 别 如 下 : 
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Ua Ug a Us 
《11a34) 
Pal 3 Bat Fs 


其 中 ,Us 和 U。 分别 表示 引力 场 和 斥 力 场 ,下 ,,, 表 
示 指 向 目标 位 置 的 引力 ,F,。 则 表示 障碍 物产 生 的 斥 
力 。. 合 力 计 算 如 图 11. 22 所 示 。 
移动 机 器 人 在 工作 空间 的 位 置 标 为 X = [zx， 
y]T, 目 标点 的 位 置 标 为 Xu 一 [zs,y。]7, 则 引力 场 
可 由 下 式 计算 
1 


Un=SR a) (11. 35) 


att 


其 中 ,参数 表示 正 引力 常数 ; U。 表示 机 器 人 所 在 
的 引力 场 。 引 力 沿 引力 场 的 负 梯 度 方 向 ,计算 如 下 : 图 11. 22 目标 点 和 陈 碍 物 对 机 器 人 
Fu 一 一 VU =k(X,a—X) (11.36) 的 合力 计算 


att goal 


由 障碍 物产 生 的 斥 力 场 Uie 为 : 


由 Fo 
了 4 人 (二 一 二 ) p< 
U dal eta C11. 37) 
0 Pp 之 oo 
其 中 ,7 是 正 标量 参数 ; o= ‖ Xo 一 X | 表示 当前 机 器 人 与 障碍 物 之 间 的 距离 ,p。 表示 预 设 
的 机 器 人 开始 受到 障碍 物 影响 时 的 距离 。 相 应 的 斥 力 可 计算 为 : 


目标 


《Lt 全 
0 ppo 
如 图 11. 23 所 示 , 当 目标 点 、 障 碍 物 和 机 器 人 的 运动 方向 在 同一 条 直线 时 ,移动 机 器 人 会 
遇 到 局 部 极 小 值 问题 , 即 移动 机 器 人 无 法 达到 目标 位 置 ,无 法 避 开 障碍 物 ,这 是 传统 PFM 的 
为 了 克服 传统 PFM 的 缺点 ,我 们 引入 了 一 种 由 MYO 手 环 确定 的 控制 命令 生成 的 虚拟 临 
时 目标 点 。 而 虚 目 标点 可 以 产生 男 一 种 引力 ,改变 移动 机 器 人 的 合力 方向 ,如 图 11. 24 所 示 ， 
从 而 使 机 器 人 重新 规划 运动 轨迹 , 避 开 障碍 物 , 到 达 目 标 位 置 。 


障碍 物 


移动 机 器 人 二 


图 11.23 目标 点 、 障 碍 物 和 机 器 人 运动 图 11.24 加 入 临时 虚拟 目标 点 后 的 合力 计算 
方向 在 同一 条 直线 


3. 基于 肌 电 信号 的 速度 控制 
这 里 利用 MYO 手 环 测量 手臂 表面 的 肌 电 信号 以 估算 人 体 的 手臂 刚度 ,并 通过 人 手臂 的 
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刚度 控制 OMR 的 速度 。 首 先 , 通 过 MYO 手 环 采集 到 的 肌 电 信号 计算 单位 时 间 内 的 均值 ,8 
个 通道 的 肌 电信 号 整合 后 计算 如 下 : 


N 
ACk) = 2 VC (11. 39) 
i=] 


ui(k) (i 二 1,2,…,NN) 表 示 MYO 手 环 第 i 个 通道 在 & 时 刻 测量 的 表面 肌 电 信号 幅 值 。 整 合 
后 的 信号 通过 平滑 滤波 可 以 得 到 : 


(11. 40) 
A(j) k>M 


其 中 ,M 王 50 表示 平滑 滤波 时 的 滑动 窗口 大 小 , 它 的 值 是 通过 试 错 法 和 滤波 效果 选取 。 
肌肉 活性 a(k) 与 肌 电信 和 号 uy (&) 之 间 的 关系 表示 如 下 : 


As 


B(k)=— OO—— (11. 41) 


其 中 ,A 为 非 线性 参数 , 取 值 范围 为 一 3 一 0。 假 设 OMR 的 运动 速度 是 根据 肌肉 刚度 计算 得 
到 。 为 了 保证 机 器 人 速度 的 稳定 性 ,在 有 限 区 间 范 围 内 需要 对 速度 进行 归 一 化 。 

在 第 个 采样 时 刻 , 机 器 人 的 速度 与 操作 者 手臂 刚度 之 间 的 关系 如 下 : 
及 (RE) 一 了 ma 


VEY = = YY pe ye Cds 2 
其 中 ,V™™ 和 V"™" 表示 OMR 运动 的 最 大 值 与 最 小 值 ， B”™ 和 B"" 表示 操作 者 手臂 刚度 的 
最 大 值 和 最 小 值 。 
11.2.3 实验 
1. 实验 设置 


为 了 验证 上 述 控制 方法 的 有 效 性 ,进行 了 三 个 实验 。 第 一 个 实验 是 移动 机 器 人 的 速度 由 
人 手臂 控制 刚度 。 为 了 比较 PF 方法 和 PFM 的 人 机 共享 控制 。 第 二 个 实验 是 当 障 碍 物 .目标 
点 和 OMR 的 方向 不 在 一 条 直线 上 时 ,OMR 达到 目标 位 置 并 通过 两 种 方法 避 开 障碍 物 。 最 后 
一 个 实验 是 通过 控制 OMR 来 避免 障碍 物 与 移动 机 器 人 的 目标 点 和 移动 方向 在 同一 条 线 上 。 

在 这 些 实验 中 ,操作 者 在 每 个 前 臂 上 都 戴 着 两 个 MYO 手 环 。 利 用 其 中 一 个 MYO 手 环 
来 估计 人 手臂 的 刚度 ,从 而 控制 移动 机 器 人 的 速度 ; 利用 另 一 个 手 环 来 识别 当 机 器 人 不 在 安 
全 距离 内 时 ,由 PFM 决定 移动 机 器 人 移动 方向 的 操作 者 的 手势 。 通 过 控制 带 有 XTION2 摄 
像 头 的 OMR 到 达 目 标 位 置 并 避 开 障碍 物 ,利用 XTION2 对 远程 环境 进行 反馈 ,测量 OMR 与 
障碍 物 之 间 的 最 小 距离 。OMR 的 初始 位 置 定义 为 (zu,yo) 王 (0,0) ,目标 位 置 为 (z。,yv ) 一 
(0,2.5)。 机 器 人 与 障碍 物 之 间 的 最 小 安全 距离 为 卫 =60cm, 三 0.1,7 王 100,A 一 一 0.42。 

2. 实验 结果 和 分 析 

在 不 同 的 阶段 ,操作 者 使 用 不 同 的 手臂 刚度 来 远程 控制 移动 机 器 人 在 平滑 的 道路 上 直行 ， 
实验 结果 如 图 11. 25 一 图 11. 32 所 示 。 图 11. 25 中 ,黑色 的 线 表 示 实 验 者 手臂 的 原始 肌 电 信 
号 ,灰色 的 线 表 示 平 滑 滤 波 后 的 肌 电 信号 。 从 图 11. 25 和 图 11. 26 可 以 看 出 ,可 以 通过 人 的 手 
壁 刚 度 有 效 控制 移动 机 器 人 的 速度 。 在 图 11. 25 中 ,人 的 手臂 在 0~6s 时 放松 ,移动 机 器 人 在 
图 11. 26 中 相应 时 间 的 速度 较 慢 。 在 6 一 16s 内 ,人 的 手臂 处 于 伸展 状态 ,机 器 人 快速 移动 ,如 
图 11. 26 所 示 。 对 比 图 11. 25 和 图 11. 26 中 不 同时 间 段 的 运动 距离 和 手臂 刚度 ,通过 提取 肌 


5 


肉 刚 度 来 估计 人 手臂 刚度 以 控制 移动 机 器 人 速度 的 方法 是 有 效 的 。 


己 


T T T T 


一 一 移动 机 器 人 移动 的 距离 


sEMG 幅 值 
距离 /m 
一 iD Uy) ~ (An CN | oo Roe) 


-800 人 0 1 
时 间 /s 时 间 /s 
图 11.25 原始 肌 电 信号 与 滤波 后 的 肌 电 信号 图 11.26 移动 机 器 人 在 不 同 手臂 刚度 下 的 运动 


由 势 场 法 控制 移动 机 器 人 在 障碍 物 .目标 点 和 运动 方向 不 一 致 时 的 运动 轨迹 如 图 11. 27 
和 图 11. 29 所 示 。 相 应 地 ,图 11. 28 一 图 11. 30 为 移动 机 器 人 向 目标 位 置 移动 时 ,障碍 物 与 移 
动机 器 人 之 间 的 最 小 距离 。 从 图 11. 27 和 图 11. 29 可 以 看 出 ,移动 机 器 人 在 人 手臂 刚 度 控 制 
下 ,移动 更 加 平稳 。 将 图 11. 28 和 图 11. 30 中 的 两 条 直线 进行 比较 ,采用 共享 控制 方法 ,障碍 
物 与 移动 机 器 人 之 间 的 最 近 距 离 更 大 ,说 明 采 用 势 场 法 和 操作 者 的 协同 控制 方法 能 够 更 快 、 更 
有 效 地 避 开 障碍 物 。 
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图 11.27 移动 机 器 人 在 势 场 法 下 避 障 轨迹 图 11.28 势 场 法 下 机 器 人 与 障碍 物 的 最 小 距离 


当 障 碍 物 .目标 点 与 移动 机 器 人 的 运动 方向 在 同一 直线 上 时 ,基于 势 场 法 由 人 控制 的 移动 
机 器 人 的 运动 轨迹 如 图 11. 31 所 示 ,障碍 物 与 移动 机 器 人 最 小 距离 的 对 应 轨迹 如 图 11. 32 所 
示 。 在 图 11. 32 中 , 当 移动 机 器 人 达到 局 部 最 小 值 时 ,移动 机 器 人 可 以 通过 虚拟 目标 位 置 改变 
移动 方向 ,因为 移动 方向 .障碍 物 和 目标 位 置 在 同一 条 线 上 。 从 图 11. 31 可 以 看 出 ,操作 者 和 
势 场 法 协同 控制 可 以 使 移动 机 器 人 有 效 避 障 。 当 目标 点 、 障 碍 物 与 移动 机 器 人 的 移动 方向 在 
同一 条 直线 上 时 ,上 述 控制 方案 可 以 克服 局 部 最 小 值 的 问题 。 
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图 11.29 移动 机 器 人 在 人 机 协作 下 避 障 轨迹 图 11.30 人 机 协作 下 与 障碍 物 的 最 小 距离 
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图 11.31 三 点 共 线 时 移动 机 器 人 的 避 障 轨迹 图 11. 32 人 机 协作 与 障碍 物 的 最 小 距离 


11.3 基于 表面 肌 电 信号 的 人 机 书写 技能 传递 


对 人 体 运 动 行为 的 研究 表明 ,在 与 动态 环境 相互 作用 的 情况 下 ,可 以 通过 调节 手臂 的 阻抗 
来 达到 稳定 ,从 而 最 大 限度 地 减少 相互 作用 力 和 运动 误差 。 受 此 研究 结果 的 启发 ,有 学 者 提出 
仿生 学 习 控 制 器 , 它 能 够 同时 适应 在 动态 条 件 下 的 力 、 阻 抗 和 轨迹 。 与 传统 的 机 器 人 控制 器 相 
比 , 它 可 以 使 得 机 器 人 以 更 低 的 成 本 具备 某 些 人 体 运 动 特征 ,因此 在 与 人 体 机 器 人 交互 中 的 相 
互 配 合 中 具有 很 大 的 潜力 。 | : 

表面 肌 电 (sEMG) 信 号 反映 人 类 关节 运动 .力量 、 刚 度 等 的 肌肉 活动 ; 并 且 ,sEMG 信号 易 
于 获取 和 快速 自 适应 响应 ,可 以 方便 用 于 不 同 的 场景 (例如 康复 、 外 上 骨骼 等 ), 同 时 还 可 以 与 力 、 
声音 或 视觉 传感器 相 结合 ,因此 广泛 用 于 理解 人 体 运 动 意图 。 

通常 ,sSEMG 信号 可 以 处 理 成 两 个 部 分 : 有 限 类 识别 序列 和 连续 控制 参考 。 前 者 通常 指 
模式 识别 ,例如 手势 识别 ,这 种 数据 库 通 常用 作 开 关 控 制 信号 ; 而 后 者 是 指 从 sEMG 信号 中 提 
取 连 续 力 、 刚 度 和 运动 序列 。 此 外 ,sEMG 与 刚度 、 力 和 运动 之 间 的 关系 近似 为 线性 ,因此 基于 
sEMG 的 机 器 人 控制 可 以 使 得 仿生 生物 控制 器 设计 变 得 简单 。 


11.3.1 人 体 臂 关节 


1. 手臂 刚度 估计 模型 
根据 力 和 sEMG 信和 号 的 近似 线性 关系 基于 sEMG 估计 人 体 手臂 末端 刚度 ,可 以 由 式 (11. 43) 
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计算 : 


Pa 
Br = Ae | . + (11. 43) 
其 中 , [As Amte] ERix2 ,忽略 机 器 人 端点 力矩 的 影响 后 ,m= 二 1; A ER”*x",A™"*E€ 
RR”*” 分 别 表示 肌肉 共同 收缩 时 激动 和 抑制 的 系数 。 此 外 ,A” 和 A” 中 的 元 素 具有 以 下 
特征 : a9 宇 0,a 有 0。F ER 及 是 由 肌肉 共同 收缩 产生 的 作用 力 ; PA 和 了 Ps。 分 别 表示 
肌肉 共 收 缩 的 激化 和 抑制 变量 ,它们 都 可 以 用 滤波 后 的 sSEMG 信号 来 表示 ; o 是 由 非 线性 因 
素 引 起 的 残余 误差 ; ”表示 所 涉及 的 肌肉 对 的 数量 。 
可 将 人 体 手臂 刚度 表示 为 式 (11. 44): 


RR 
Ki = A "| 1 | 十 ca (11. 44) 


anta 


其 中 ,KE 尺 扣 ”表示 通过 骨骼 肌肉 参与 产生 的 终点 刚度 ，[A 各 Ai |] 中 的 元 素 是 


[A” Asae] 的 绝对 值 ; c 是 非 线性 的 残余 误差 和 固有 刚度 。 
为 了 克服 非 线 性 残 差 带 来 的 影响 ,人 体 手臂 刚度 增 量 的 简化 模型 刚度 可 以 由 式 (11. 45) 
表示 。 


rd a ed Ss 
i=1l trl 


其 中 ,A”” “是 检测 到 的 第 i 个 在 当前 时 刻 t 的 持 抗 肌 的 sSEMG 的 振幅 , 即 和 AA” “是 收缩 盘 
肌肉 的 所 由 * 改 及 AA 二 A A 0 
AK， 是 笛 卡 儿 空 间 中 末端 刚度 的 矢量 ,因此 需要 映射 机 器 人 关节 空间 。 

2. 使 用 平方 和 低 通 滤波 方法 对 sSEMG 信号 进行 处 理 

sEMG 信号 是 在 肌肉 活动 时 产生 的 非 侵 入 性 信号 ,代表 肌肉 张力 、 关 节 力 和 刚度 变化 等 。 
它 是 由 运动 单位 触发 的 运动 单位 动作 电位 (Motor Unit Action Potentials, MUAPs) 链 的 复合 
线性 求 和 。 通 常 , 提 取 20~500Hz 的 sEMG 信号 用 于 后 处 理 ,而 400 一 500Hz 范围 内 的 信号 
与 所 有 肌肉 的 刚度 均 相 关 , 且 只 有 1% 的 sEMG 信号 有 助 于 估计 平均 力 。 因 此 ,400 一 500Hz 
的 信号 足以 进行 刚度 估计 。 另 外 ,从 sEMG 估计 的 刚度 应 该 进行 平滑 处 理 , 以 获得 更 好 的 机 
器 人 性 能 。 

采用 基于 抽样 和 低 通 滤波 的 信号 包 络 算法 来 提取 式 (11. 46) 中 所 示 的 估计 刚度 的 包 络 
振幅 。 


一 1 
P(s)=PO XLAM — nj].hLn).f (fs,X).ZOHG.) (11. 46) 
1 一 0 


其 中 ,P(s) 表 示 经 过 平方 和 低 通 滤波 器 后 的 sSEMG 信号 的 幅度 ; P 是 SEMG 信号 平方 系数 ，; 
n 是 脉冲 函数 [nj 的 长 度 ; M 是 第 M 个 输出 ; X(。) 是 预 滤波 的 SEMG 信号 ; FCFs,X) 是 
对 应 于 下 采样 的 低 通 滤波 器 ; ZOH (1 ) 是 零 阶 保持 器 ,用 于 生成 与 参考 位 置 相同 的 数据 维度 。 
提取 P(z) 的 包 络 线 , 并 利用 其 增 量 式 计量 学 来 估计 增 量 刚度 。 图 11. 33 给 出 了 使 用 平方 和 低 
通 滤波 方法 包 封 的 SEMG 信号 的 示例 。 


K K 
AP() 一 >)PG 十 1) 一 >)PC) CI dy 


k=0 k=0 
如 图 11. 34 所 示 为 端点 刚度 估计 ,其 含义 如 表 11. 1 所 示 。 整 体 增 量 刚度 系数 估计 如 
图 11. 35 所 示 。 
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0.15 T T T 


原始 EMG 信 号 


4 5 
时 间 /s 


时 间 /s 
图 11. 33 使 用 平方 和 低 通 滤波 方法 对 sEMG 信号 进行 处 理 示 例 


“3 BILHD ?2 DELC 
全 1 BRAD ~ 
1_PMJC 


STRIM 7 TRIO 
图 11.34 所 涉及 的 电极 的 肌肉 .数量 和 位 置 在 估计 端点 刚度 


表 11.1 参与 肌 电 图 刚度 估计 的 肌肉 形态 


肩 弯 曲 , 水 平 内 收 等 
肩 水 平 内 收 、 旋 转 、 内 收 
屈 肘 


肩 部 伸展 、 水 平 外 展 、 外 旋 
肘 关节 伸 直 
肘 关节 伸 直 
肘 关节 伸 直 / 肩 伸展 


3. 人 体 手 名 刚 度 映射 到 机 器 人 关节 
通过 笛 卡 儿 空 间 到 关节 空间 增 量 映射 ,可 以 得 到 手臂 末端 的 增 量 刚 度 , 即 : 


-Daa 十 ko (11. 48) 


其 中 ,ko = 二 diag (ko11 ,kz2 ,Er  ) 是 关节 刚度 矩阵 ， 每 个 对 角 线 元 素 对 应 于 样本 时 间 的 单 
个 关节 。Ak 二 diag(Ak', ,Ak', ,ARL ,0,0,0) 表 示 增 量 笛 卡 儿 末 端的 刚度 ,其 中 对 角 元 素 是 
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原始 SEMG 信 和 号 


处 理 后 的 
sEMG 信 号 
信号 分 解 带 宽 
包 络 

序列 |4(m)| 


力 传 感 器 


图 11.35 ” 增 量 刚度 系数 估计 流程 图 


从 式 (11. 45) 中 获得 的 。J' ER"" 是 雅 可 比 和 矩阵。 本 节 中 n= 二 7, 因 为 Baxter 机 器 人 的 一 个 辟 

有 7 个 关节 。k,o。€R'” 对 角 和 矩阵 的 每 个 对 角 线 元 素 对 应 的 单个 关节 对 应 是 Baxter 机 器 人 的 

初始 刚度 。 此 外 ,机 械 辟 的 刚度 需要 保持 在 合理 范围 内 。 
因此 ,可 以 使 用 式 (11. 49) 来 合并 指定 的 边界 。 

Ch = 
其 中 ,k,;; 表示 基于 sEMG 的 估计 修正 刚度 值 ; &?” 是 估计 的 最 大 值 刚度 ; An 是 估计 的 最 
小 值 刚度 ; &y 是 估计 的 关节 刚度 值 。 

11.3.2 实验 设计 
1. 机 器 人 实验 系统 简介 
本 节 所 用 实验 平台 为 Baxter 机 器 人 ,如 图 11. 36 所 示 ,其 机 械 臂 的 每 个 接头 由 一 个 串 行 

弹性 致 动 器 (SEA) 驱 动 ,提供 被 动 顺 应 性 ,以 实现 任何 接触 或 冲击 的 力 的 最 小 化 。 本 实验 使 用 
基于 分 布 式 控制 系统 的 通信 系统 是 Robot Raconteur( 简 
称 RR, 使 用 机 器 人 Raconteur 版 本 0.4 测试 ,如 图 11. 37 
所 示 )。 机 器 人 Raconteur 允许 用 户 访问 数据 ,例如 关节 
力矩 .位 置 双 边 速 度 ,可 以 通过 MATLAB、C++、Python 
等 语言 进行 编程 。 此 外 , RR 适用 于 多 个 平台 ,兼容 
Windows 和 Linux, 可 以 在 不 同 的 传感器 和 操作 系统 之 间 
进行 通信 。、 

2. 增 量 刚度 估计 和 书写 节能 传递 实验 设计 


在 执行 书写 技能 传递 任务 之 前 ,需要 先 实现 以 下 3 个 
步骤 。 图 11.36 Baxter 机 器 人 


kas = (he — Re ) > (11. 49) 
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Baxter 机 器 人 | 


NI Matlab/ 
USB6210 Simulink 
图 11.37 Baxter 的 远程 通信 系统 设计 


(1) 首先 需要 对 人 体 手臂 刚度 进行 校准 ,如 图 11. 38 所 示 。 人 从 到 机 器 人 的 刚度 映射 设 
计 及 基于 阻抗 控制 的 交互 如 图 11. 39 所 示 ,要 求 在 人 的 手腕 附近 士 X 、 士 Y、 士 2 方向 和 以 一 定 
的 耦合 机 制 选 取 的 随机 方向 移动 ,以 限制 其 运动 。 
如 图 11. 39 所 示 ,通过 施加 士 5N、 士 1ON 、 士 15N 的 
力 ,使 用 NIUSB6210、8 通道 的 sEMG 信和 号 放大 器 
和 MATLAB 2014a 32 位 数据 采集 工具 同步 记录 ， 
采样 率 为 2kHz, 可 以 在 每 个 坐标 轴 上 获得 增 量 矢 
量 和 相应 的 sEMG 信号 。 在 端点 刚度 估计 中 涉及 
8 个 肌肉 ,并 且 应 该 进行 至 少 8X3 次 试验 以 计算 在 
第 卡 儿 空间 中 在 3 个 平移 方向 上 估计 端点 刚度 所 
需 的 所 有 系数 。 本 节 进 行 了 54 次 试验 , 即 3( 分 别 


机 器 人 
Raconteur(RR) 


施 力 5N 10N .15N)X3(3 次 重复 试验 ) X2( 一 个 坐 耦合 机 制 
标 轴 的 2 个 方向 ,例如 士 X。.)X3(3 个 坐标 轴 X、 图 11. 38 基于 力 传感器 和 肌 电 信和 号 


Y、Z) ,在 土 X、 士 Y、 士 Z 的 每 个 方向 上 进行 实验 ， 的 刚度 校准 
并 利用 最 小 二 乘法 提高 估计 精度 。 

(2) 将 人 体 手 臂 末 端的 刚度 映射 到 机 械 臂 的 关节 空间 上 。 

(3) 阻抗 接口 设计 : 所 有 的 比较 测试 均 基 于 阻抗 设计 ,人 体 臂 端点 运动 将 作为 参考 轨迹 并 
通过 Baxter 位 置 传感器 记录 ,映射 刚度 为 反馈 回路 中 位 置 误差 的 增益 ,速度 增益 由 式 (11. 50) 确 
定 。 阻 抗 接口 为 PD 控制 句 , 见 式 (11. 51): 

ks (11. 50) 


qii 
站 二 一 诡 视 ) 一 okGi 1 ) 人 开工: 六 
其 中 ,q, 和 人， 分 别 表示 参考 轨迹 与 参考 速度 。 
Baxter 
机 器 人 


增 量 化 
刚度 
提取 


阻抗 
控制 顷 


图 11.39 从 人 到 机 器 人 的 刚度 映射 设计 及 基于 阻抗 控制 的 交互 
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11.3.3 人 -机 器 人 技能 传递 
首先 ,人 教 机 器 人 完成 书写 任务 : 使 用 一 个 耦合 模块 将 一 个 主体 (右手 ) 右 手腕 与 Baxter 
一 个 机 械 辟 末端 耦合 。 同 时 , 示 教 者 将 sEMG 手 环 
佩戴 在 手臂 上 以 记录 sEMG 信号 ,如 图 11. 40 所 示 ， 
并 记录 sEMG 信号 、Baxter 机 器 人 末端 位 置 以 及 末 
端 执 行 器 所 受到 的 力 以 进一步 分 析 。 示 教 者 用 右手 
抓 住 Baxter 机 器 人 末端 执行 器 在 纸 上 写 下 “ 口 ”。 
为 了 对 比 , 这 里 进行 了 3 次 测试 。 
(1) Baxter 机 器 人 在 恒定 高 刚度 模式 下 完成 书 
写 任务 (力矩 分 别 为 800N/m、800N/m、800N/m); 
(2) Baxter 机 器 人 在 恒定 低 刚 度 模 式 下 完成 书 图 11.40 人 -机 器 人 书写 技能 传递 实验 设置 
写 任 务 (力矩 分 别 为 100N/m、100N/m、100N/m); 
(3) Baxter 机 器 人 在 变 刚 度 控制 模式 下 完成 书写 任务 ,如 图 11. 41 所 示 。 图 11. 42 和 
图 11. 43 所 示 为 从 sEMG 信号 提取 的 高 刚度 、 低 刚度 和 变 刚 度 下 的 端点 接触 力 和 位 置 误差 。 
图 11. 44 表示 为 当 人 示 教 机 器 人 书写 时 ,相应 的 接触 力 与 记录 的 接触 力 之 间 的 相关 性 。 


(a) 参考 书写 轨迹 (b) 高 刚度 书写 (Cc) 基于 sEMG 的 刚性 书写 (qd) 低 刚度 书写 


图 -了 1,4#1 写作 结 


妆 
斑 9 
i 
区 -和 1 1 1 
i 
山名) 在 村 定 高 刚度 模式 下 书写 ( 力 逢 分 别 为 600N/m、800N/m 、800N/m)， 
开 0.04 
世 0.02 
喜 0 
S 了 他 1 1 1 1 1 
0 05 1.0 LA 二 
有 ee i ere gd X104 
(b) 通过 SEMG 在 可 变 刚度 模式 下 书写 信号 和 
并 0.04 
瑟 0.02 
-0.04 1 1 1 1 1 
i 1.0 人 
加 X104 
(c) 在 恒定 低 刚 度 模式 下 书写 (力矩 分 别 为 100N/m、100N/m、100N/m) 
| | 
Pp、 Pp, Pp, 


图 11.42 .写作 技能 结果 比较 : 三 种 不 同 的 端点 刚度 情况 的 位 置 误差 
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和 0 00753073 
(a) 参考 写作 


lo 05 10 15 20 2.5 30 3.5 4X104 
(b) 高 刚度 (力矩 分 别 为 800N/m、800N/m、800N/m) 
10 


力 N 


DY OD 1S° F020 25% FO Ts: 4X 1 
(c) 从 sEMG 信 号 中 提取 的 可 变 刚度 


0 TS HOS FOF dX 
(d) 低 刚 度 ( 力 矩 分 别 为 100Nm、100Nm、100N/m) 


允许 将 机 器 人 | 
移动 到 写作 区 域 | 
Fee ,ee F， 实 施 写 作 任务 ”完成 


图 11.43 写作 技能 结果 比较 : 三 种 不 同 端 点 刚度 场景 的 接触 力 


0.50 


高 刚度 ” ”ssEMG 变 刚度 。 ””” 低 刚度 - 
图 11. 44 ”接触 力 在 Z 方向 上 与 参考 力 的 关系 


11.4 基于 和 触觉 反馈 的 Baxter 机 器 人 遥 操 作 控制 


本 节 将 介绍 一 种 具有 新 特征 的 遥 操 作 方法 ,使 得 操作 人 员 可 以 使 用 物理 和 生理 手段 操控 
远程 机 右 人 。 

大 多 数 遥 操作 系统 采用 主 从 框架 ,将 在 从 端 采样 的 反馈 信息 送 回 主 端 ,使 得 主 端 操 作 人 员 
可 以 根据 这 些 反 馈 信息 实时 操控 远 端 机 器 人 。 作 为 人 类 操作 者 和 远 端 机 器 人 的 组 合 , 遥 操 作 
机 器 人 系统 应 该 融合 机 器 人 和 操作 员 的 智能 以 实现 最 佳 性 能 。 但 是 大 多 数 现 有 技术 忽略 了 这 
一 点 ,并 且 没 有 考虑 根据 每 个 人 的 运动 行为 来 调整 机 器 人 控制 器 。 生 理学 家 已 经 建立 了 许多 
合理 的 人 体 运 动 控制 数学 模型 ,机 器 人 专家 也 开发 了 许多 先进 的 控制 技术 ,但 很 少 有 人 努力 将 
两 个 领域 的 专业 知识 结合 起 来 ,开发 出 完美 融合 人 类 和 机 器 人 智能 的 匹配 控制 技术 。 

人 体 运动 学 相关 研究 表明 ,人 体 手 辟 能够 调节 肌肉 群 的 共同 收缩 ,在 与 动态 环境 的 交互 过 
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程 中 产生 所 需 的 骨架 机 械 阻 抗 , 从 而 最 大 限度 地 减少 相互 作用 力 和 性 能 误差 。 因 此 ,理想 的 方 
法 是 将 人 的 自 适 应 阻抗 特征 传递 给 远 端 机 器 人 。 

传统 方法 不 能 完全 捕获 和 转移 人 类 操作 者 的 运动 技巧 。 由 附着 在 人 体 皮肤 上 的 非 侵 入 性 
电极 收集 表面 肌 电 图 (SEMG) 信 和 号 是 将 人 类 运动 技巧 结合 到 机 器 人 中 的 理想 生理 手段 。 它 们 
反映 了 代表 人 体 关节 运动 、 力 和 刚度 的 人 体 肌 肉 活动 。 实 际 上 ,sEMG 信和 号 已 广泛 用 于 机 器 人 
在 执行 任务 期 间 理 解 人 体 运 动 意图 ,并 且 最 近 也 被 用 于 远程 阻抗 控制 。 阻 抗 调节 在 提高 稳定 
性 、 准 确 性 和 任务 准备 性 等 方面 有 重要 作用 ,因此 ,将 肌肉 阻抗 适应 性 引入 遥 操 作 系统 可 提高 
操作 远 端 机 器 人 的 灵活 性 。 此 外 ,将 触觉 控制 系统 引入 触觉 界面 ,该 触觉 界面 通过 力 反馈 为 操 
作者 提供 远程 环境 的 触觉 感受 ,并 且 将 力 反馈 引入 远程 操作 系统 可 以 有 助 于 减少 能 耗 、 任 务 完 
- 成 时 间 和 误差 。 

11.4.1 遥 操 作 控 制 系 统 

本 节 中 介绍 的 遥 操 作 系统 采 用 主 从 控制 结构 , 主 端 使 用 触觉 设备 (Sensable haptic 
Omni) ,从 端 设备 主要 由 七 自由 度 Baxter 机 器 人 组 成 。 对 机 器 臂 的 位 置信 息 进 行 采样 ,并 发 
送 到 中 央 处 理 计算 机 ,同时 将 反馈 力 施加 在 触觉 设备 的 触 笔 上 。 以 这 种 方式 ,操作 人 员 可 以 根 
据 触觉 反馈 和 视觉 反馈 来 操作 远 处 的 物体 。 整 个 遥 操 作 系 统 的 结构 如 图 11. 45 所 示 , 由 
11. 48(a) 中 所 示 的 Omni Haptic 所 收集 的 物理 信号 (位 置 ) 和 图 11. 46 所 示 的 MYO 手 环 
收集 的 生理 信号 (sEMG) 集 成 在 一 起 ,以 产生 控制 远程 机 器 人 机 械 臂 的 命令 。 


通过 蓝牙 


主机 客户 端 


Omni Baxter 
Haptic 机 器 人 


图 11.45 遥 操 作 系 统 的 结构 


1，MYO 手 环 

sEMG 信号 可 以 被 视 为 由 运动 单元 触发 的 运动 单元 动作 电位 (MUAP) 发 送 的 复合 物 的 
线性 信和 号。 这 里 使 用 无 线 N=8 通道 EMG 信号 装置 MYO ,默认 采样 频率 为 200Hz。 与 传统 
电极 相 比 , MYO 手 环 佩戴 更 容易 , 它 由 8 个 EMG 传感器 和 9 轴 IMU( 惯 性 测量 单元 ) 组 成 。 

2. Baxter 机 械 臂 

Baxter 机 器 人 由 安装 在 可 移动 基 座 上 的 躯干 和 分 别 安装 在 左 / 右 臂 安装 座 上 的 两 个 七 自 
由 度 手臂 组 成 。 每 个 臂 有 7 个 旋转 接头 和 8 个 连 杆 ,以 及 可 安装 在 每 个 臂 末 端的 夹具 (例如 电 
动 夹具 或 真空 杯 )。 基 于 Denavit-Hartenberg(DH) 参 数 的 Baxter 机 器 人 运动 学 模型 前 面 章 
节 已 经 有 建 模 。 从 端 Baxter 机 器 人 有 两 个 相同 的 七 自由 度 (DOF) 机 械 臂 ,如 图 11. 47 所 示 。 
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Le 


11.46 MYO 手 环 图 11. 47 Baxter 机 械 臂 运动 学 模型 


3. Omni 触觉 操纵 杆 

SensAblOmni 触觉 操纵 杆 如 图 11. 48(a) 所 示 ,用 于 产生 力 反 馈 。 该 触觉 装置 具有 6 个 自 
由 度 ,其 中 前 3 个 自由 度 处 理 位 置 ,后 3 个 自由 度 形成 有 助 于 定向 的 万 向 节 。 配 有 两 个 按钮 的 
手写 笔 也 连接 到 末端 执行 器 。Omni 装置 的 运动 学 包括 正 向 运动 学 、 逆 向 运动 学 和 雅 可 比 运 
动 学 。 


(a) Omni 触 觉 操 纵 杆 (b) 力 反馈 生成 器 
图 11. 48 SensAblOmni 触觉 操纵 杆 


本 节 不 使 用 安装 在 机 械 臂 上 的 力 传感器 来 传递 测量 的 相互 作用 力 。 相 反 , 采 用 基于 运动 
跟踪 误差 信息 的 触觉 泻 染 算法 ,使 用 导入 模型 生成 力 反馈 ,如 图 11. 48(b) 所 示 , 遵 循 以 下 
规定 : 

F=MdX+DdX++KdX (11. 52) 
其 中 ,K 是 虚拟 弹簧 的 刚度 ,D 是 虚拟 阻尼 器 的 阻尼 比 ,M 是 虚拟 质量 ,dX 是 Baxter 末端 执 
行 器 的 实际 位 置 与 Omni 操纵 杆 设 定 的 指令 参考 位 置 之 间 的 差 值 。 为 简单 起 见 ,只 考虑 3D 平 - 
移 运 动 ,并 且 dX = 一 X, 一 X, ,其 中 X.ER” 的 是 Baxter 机 器 人 末端 执行 器 的 实际 平移 位 置 ， 
X ER 是 由 主 Omni 设备 设置 的 参考 位 置 。 

根据 式 (11. 52) 生 成 的 反馈 力 可 以 增强 人 类 操作 者 的 跟踪 性 能 意识 ,即将 触觉 感 测 与 视觉 
感 测 结合 在 一 起 。 同 时 ,操作 者 的 肌肉 激活 将 在 潜意识 中 被 放大 以 产生 抵抗 力 。 然 后 ,控制 增 
益 将 相对 应 于 增长 的 EMG 信号 而 增加 ,使 得 机 器 人 可 以 很 好 地 跟随 操作 者 的 运动 。 

4. 工作 区 匹配 

对 于 与 主 设备 在 运动 学 上 不 同 的 远程 机 器 人 ,在 操作 时 应 该 记 住 遥控 机 器 人 机 械 辟 在 其 
自己 的 工作 空间 中 工作 ,该 工作 空间 可 能 与 主 设备 的 工作 空间 完全 不 同 。 

因此 ,评估 给 定位 置 是 否 可 达 是 一 个 基本 问题 。 分 析 方 法 可 以 确定 工作 空间 边界 的 闭合 
形式 描述 ,但 这 些 方法 通常 由 机 械 臂 运动 学 中 涉及 的 非 线 性 方程 和 矩阵 求 逆 而 复杂 化 。 另 一 
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方面 ,数值 方法 相对 更 有 效 。 蒙 特 卡 罗 随 机 抽样 数值 方法 , 仅 使 用 正 向 运动 学 生成 一 些 简单 机 
械 臂 的 工作 空间 边界 。 该 方法 应 用 起 来 相对 简单 ,这 里 通过 
它 创 建 工作 空间 映射 模型 。 

Omni 操纵 杆 的 坐标 框架 轴 的 定义 与 Baxter 的 不 同 ,如 
图 11. 49 所 示 。 因 此 ,Omni 的 笛 卡 九 坐标 [x y% xz%]” 
需要 根据 下 面 的 等 式 修改 : 


0 0 
A'=R. (3 )R, (3)4.R, (三 je 二) 。 站 
0 1 图 11.49 Omni 和 Baxter 的 


(11. 53) 框架 轴 方 向 


其 中 ,R,\、R, 和 R. 是 旋转 力矩 阵 ,4， 是 Omni 的 变换 和 矩阵 ,A' 是 相应 的 修改 矩阵 。 
根据 主 端 设备 和 从 端 机 器 人 的 正 向 运动 学 和 关节 旋转 限制 ,对 机 械 辟 的 关节 空间 使 用 蒙 
特 卡 罗 随 机 采样 方法 ,以 逼近 主 从 机 构 的 工作 空间 。 采 用 均匀 径 向 分 布 在 主 从 关节 空间 分 
别 生 成 8000 个 点 。 为 了 使 主 Omni 操纵 杆 的 工作 空间 和 从 属 Baxter 机 器 臂 的 工作 空间 尽 
可 能 地 相互 重 蚕 以 提高 可 操作 性 ,使 用 点 云 匹配 方法 。 考 虑 到 末端 执行 器 的 位 置 ,映射 处 


理 如 下 : 
i co 一 Sb 0 .> 7 0 | |Zm 上 
y, | 一 |sing cosd | [0 3 | gal (11..54» 
区 0 0 1 0 0 “S| | ' 


其 中 ,[x、 y， z,]' 和 [zx,， y, >,] 分 别 是 Baxter 和 Omni 末端 执行 器 的 笛 卡 儿 坐标 ， 
d 是 关于 Baxter 基 帧 的 z 轴 的 旋转 角度 ,[S。 S， S.J" 和 [LT。 T，T-.]: 是 关于 zy、 
z 轴 的 比例 因子 和 平移 长 度 。 

对 于 Baxter 机 器 人 的 左 臂 , 式 (11. 54) 中 的 参数 如 下 : 


5 和 

。 | 
S 0. 040 
S -| 0. ma 


0.701 
0. 210 
0 二 


工作 空间 匹配 的 结果 在 3D 空间 中 的 表示 如 图 11. 50 所 示 。 此 外 ,触觉 操纵 杆 的 轴 的 方 
向 与 远程 机 器 人 的 轴 的 方向 不 同 。 

注意 ,上 述 工 作 空间 匹配 导致 运动 放大 ,因为 Baxter 机 器 人 手臂 的 工作 空间 在 物理 上 远 
大 于 Omni 操纵 杆 的 工作 空间 。 这 种 放大 会 增加 精细 操作 的 难度 ,例如 ,用 户 的 微小 运动 误差 
将 导致 远程 机 器 人 大 的 非 期 望 的 运动 偏差 。 


11.4.2 肌肉 活动 提取 


从 sEMG 到 肌肉 激活 的 转变 是 这 项 工作 的 重要 过 程 ,如 何 从 sEMG 估计 激活 水 平 对 本 工 
作 中 提出 的 可 变 增益 控制 的 性 能 影响 很 大 。 首 先 ,来 自 所 有 通道 的 原始 sSEMG, 即 wu; CR) ,其 
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1.5 =1.0 


图 11.50 工作 区 匹配 结果 , 深 色 点 状 云 是 从 属 的 工作 空间 , 浅 色 点 状 云 代 表 主 工作 空间 


中 & 是 当前 采样 时 刻 ,i 二 1,2,… ,NN ,以 如 下 均 方 根 方式 集成 在 一 起 : 


a(k) = >) Mu? (k) (11. 55) 
i 一 1 
滑动 平均 滤波 器 如 下 : 
1 k+M 
ee /RE) (11. 56) 
其 中 ,M 一 20。 
采用 从 神经 激活 ulk) 到 肌肉 激活 <(&) 的 非 线性 映射 ; 
Aul(k) _ 
se (11. 57) 
| 


其 中 ,A 是 非 线性 因子 。 


11.4.3 基于 sSEMG 的 变 增 益 控制 

1. 控制 增益 计算 

假定 控制 增益 与 肌肉 活动 之 间 为 倍数 关系 。 然 而 ,将 控制 增益 在 规定 的 稳定 运动 范围 内 
归 一 化 是 很 重要 的 ,否则 ,由 于 控制 速率 和 与 人 体 刚度 变化 的 不 相 容 性 , 它 可 能 导致 不 稳定 。 
式 (11. 58) 用 于 通过 合并 指定 范围 在 第 个 采样 时 刻 产 生 控制 增益 : 


min ) (a(k)—a™) ， 


Gain(k) = (Gain™ — Gaim 十 Gain (ll.58) 


Ca max __ - min ) 


其 中 ,稳定 的 机 器 人 运动 增益 范围 最 大 增益 Gain"* 和 最 小 增益 Gain™" 以 及 最 大 和 最 小 肌肉 
激活 a™ 和 am" 可 以 事先 通过 实验 获得 。 

2. 控制 器 设计 

变 增 益 控 制 是 在 Baxter 机 器 人 提供 的 位 置 控 制 模式 和 力矩 控制 模式 下 实现 的 ,控制 模式 
如 图 11. 51 所 示 。 其 中 ,z ”是 操作 员 指 令 的 参考 轨迹 ,z 是 机 器 人 的 实际 轨迹 ,J 是 机 械 臂 的 
雅 可 比 行列 式 ,J “是 J 的 伪 逆 。 在 图 11. 51(a) 所 示 的 位 置 控制 模式 中 ,由 反映 肌肉 活动 的 
sEMG 产生 的 控制 增益 直接 影响 要 发 送 到 Baxter 机 器 人 的 位 置 控制 器 的 指令 速度 。 因 此 ,在 
该 控制 模式 中 ,操作 者 能 够 通过 肌肉 收缩 来 调节 机 械 臂 的 运动 速度 。 图 11. 51(b) 所 示 为 力矩 
控制 模式 结构 图 ,而 指定 的 控制 增益 将 以 一 定 比例 倍数 影响 从 端 机 械 辟 的 刚度 。 
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(b) 力矩 控制 模式 
图 11. 51 两 种 控制 模式 的 结构 


11.4.4 ”实验 设计 

本 节 设 计 两 组 实验 ;以 验证 sEMG 增强 遥 操 作 系统 的 有 效 性 。 

1. 位 置 控制 模式 下 实验 设计 1 

在 位 置 控制 方法 中 ,可 以 设置 控制 增益 以 改变 遥控 机 器 人 的 跟随 速度 。 设 计 了 一 个 抓 取 
和 放下 任务 来 测试 基于 EMG 的 可 变 增 益 控 制 方法 的 性 能 。 在 该 实验 中 ,操作 者 将 从 目标 上 
用 框 中 “十 ” 字 标 记 的 起 始 位 置 抓 取 目标 对 象 到 表格 上 标记 为 “又 ”的 目标 位 置 ,如 图 11. 52(b) 


所 示 。 


E (a) 接送 任务 (b) 提升 和 移动 任务 
图 11.52 位 置 控制 模式 和 力矩 控制 模式 下 的 变 增益 控制 策略 的 实验 设计 


实验 中 抓 取 对 象 为 如 图 11.52(a) 所 示 的 绿色 物体 。 从 测试 期 间 记 录 的 视频 剪辑 中 捕获 
图 11. 53 一 图 11.55。 图 11.53(a) 图 11.54(a) 和 图 11.55(a) 为 机 械 臂 到 达 物 体 时 的 情况 ; 
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图 11.53(b)、11.54(b) 和 图 11.55(b) 为 机 械 臂 抓 住 物体 并 开始 移动 到 期 望 目 标 下 落 位 置 时 的 
状态 ,如 在 桌子 上 用 “十 ” 字 标 记 的 那样 。 图 11. 53(c) .11. 54(c) 和 图 11. 55(c) 所 示 为 机 械 臂 
到 达 下 落 位 置 的 情况 ; 图 11. 53(d) 、11. 54(d) 和 图 11. 55(d) 为 机 械 臂 下 落 物 体 并 开始 移 回 原 
始 位 置 的 情况 。 低 增益 模式 和 基于 EMG 的 可 变 增益 模式 的 操作 员 可 以 平滑 且 准 确 地 到 达 和 
降低 目标 。 在 高 增益 控制 模式 下 ,未 经 训练 的 操作 者 很 难 在 第 一 时 间 准 确 地 抓 住 物体 ,因为 任 
何不 准确 的 操作 都 会 被 迅速 跟踪 和 放大 。 图 11. 53(b)、(c) 中 用 圆圈 突出 显示 的 物体 已 经 被 
机 械 臂 与 目标 位 置 分 开 , 即 在 桌子 上 标记 “又 ?”。 通 常 ,当主 设备 的 工作 空间 远 小 于 从 机 器 人 的 

工作 空间 时 ,运动 放大 。 当 遥控 机 器 人 放大 操作 者 的 微小 运动 时 ,操作 者 很 难 在 高 增益 模式 下 
完成 精细 的 操作 。 


图 11.53 高 


六 
ea 


肉 模 式 


(b) (c) 
图 11.54 低 增益 模式 


(b) (©) 
图 11.55 基于 sEMG 的 可 变 增益 模式 


在 高 增益 模式 和 基于 EMG 的 可 变 增 益 模式 下 ,整个 抓 取 和 放下 任务 的 时 间 消 耗 相对 较 
低 , 而 在 低 增益 模式 下 则 高 得 多 。 对 比 实 验 表明 ,位 置 控制 模型 中 基于 sEMG 的 可 变 增益 确 
保 了 高 效 .平滑 和 准确 的 操作 。 与 高 增益 和 低 增 益 模式 相 比 , 它 可 以 带 来 更 好 的 用 户 体验 , 特 
别 是 对 于 未 经 训练 的 ,不 熟练 的 操作 员 。 

geal holo a 11. 56(a) 所 示 。 图 11. 56(b) 中 , 绿 线 表 示 移 动 平 均 滤 波 后 
的 原始 EMG 信号 , 蓝 线 是 检测 到 的 包 络 线 , 表 示 从 EMG 信号 中 提取 的 肌肉 动作 。Omni 操 
纵 杆 提供 的 力 反馈 如 图 11. 56(c) 所 示 。 图 11. 56 表明 , 当 采 用 基于 sEMG 的 可 变 增益 控制 模 
式 时 , 当 操 作者 受到 反馈 力 时 ,机 械 臂 的 速度 将 增加 ,反之 亦 然 。 
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图 11.56 基于 sEMG 的 可 变 增益 模式 下 力 反 馈 和 机 器 人 跟踪 速度 之 间 的 关系 


2. 在 力矩 控制 模式 下 测试 

在 刚度 控制 方法 中 ,可 以 设 定 机 械 臂 的 刚度 以 调节 机 械 臂 的 力矩 。 提 升 和 移动 任务 旨 在 
验证 基于 EMG 的 可 变 刚 度 是 否 能 够 在 效率 和 准确 性 方面 提高 任务 的 性 能 。 在 该 实验 中 ,要 
求 操作 员 将 目标 物体 从 左 侧 桌子 上 用 十 字 标 记 的 起 始 位 置 抬 起 到 右 侧 桌子 上 用 十 字 标 记 的 目 
标 位 置 , 如 图 11.53(b) 所 示 。 

如 图 11.53(b) 所 示 ,重型 工具 箱 是 提升 和 移动 的 对 象 。 当 施加 高 刚度 时 ,工具 箱 可 以 成 
功 提 升 。 当 箱 体 被 抬 起 时 ,机械 臂 的 动力 学 突然 改变 ,会 导致 机 械 臂 不 稳定 ,如 图 11. 57(a) 所 
示 。 当 施加 低 刚 度 时 ,如 图 11. 57(b) 所 示 ,物体 根本 没有 升力 ,并 且 几 乎 不 能 被 拖 到 目标 位 
置 。 当 应 用 基于 sEMG 的 变 刚 度 控制 策略 时 ,如 图 11. 57(c) 所 示 ,机 械 臂 能 够 提升 物体 并 保 
持 其 自身 稳定 。 图 11. 57(d) 显示 了 三 种 不 同 模式 下 跟踪 的 均 方 根 误差 (RMSE) ,由 图 可 见 基 
于 EMG 的 可 变 刚 度 控 制 模式 可 获得 最 低 的 RMSE。 

在 图 11. 58(a) 中 ,曲线 表示 机 械 臂 的 刚度 百分比 。 在 图 11. 58(b) 中 ,变化 较 剧烈 的 曲线 
表示 EMG 信号 的 原始 数据 ,变化 较 平 缓 的 曲线 是 滤波 的 EMG 信号 。Omni 的 力 反 馈 如 
图 11. 58(c) 所 示 。 图 11. 58 表明 : 在 基于 sEMG 的 可 变 刚 度 控制 模式 下 , 当 操 作者 前 臂 上 的 
反馈 力 增加 时 ,机 械 辟 的 刚度 值 将 增加 ,反之 亦 然 。 
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图 11.57 跟踪 性 能 ; 灰 线 表示 由 Omni 操纵 杆 设 定 的 位 置 轨迹 , 黑 线 机 械 辟 的 实际 位 置 轨迹 


刚度 /% 
S 


0 1000 2000 3000 4000 5000 6000 7000 8000 9000 10000 11000 
时 间 /ms 
(a) 刚度 估计 


原始 EMG 
oo 包 络 EMG 


0 1000 2000 3000 4000 5000 6000 7000 8000 9000 .10000 11000 
时 间 /ms 
(b) 肌 电信 号 ; 灰 线 为 移动 平均 滤波 后 的 原始 EMG 信 号 ， 黑 线 为 灰 线 的 包 络 线 


图 11.58 基于 sEMG 的 变 刚度 模式 下 力 反馈 与 机 器 人 跟踪 速度 之 间 的 关系 
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图 11.58 ( 续 ) 


11.5 基于 人 体 运 动 跟踪 的 Baxter 机 器 人 遥 操 作 控 制 


本 节 将 介绍 基于 Kinect 传感器 ,采用 矢量 法 和 逆 运 动 学 方法 对 机 器 人 进行 遥 操 作 控 制 。 

为 了 实现 人 体 动 作 跟踪 ,首先 应 该 跟踪 人 体 本 身 。 目 前 ,有 许多 人 体 跟 踪 方法 ,最 常用 的 
方法 是 将 跟踪 标记 固定 在 人 体 上 ,但 这 可 能 会 给 用 户 带 来 很 多 不 便 。 另 一 种 方法 是 使 用 普通 
相机 的 图 像 处 理 。 由 于 图 像 处 理 系统 对 于 身体 的 检测 能 力 差 ,这 种 方法 不 可 靠 。 使 用 立体 视 
党 相机 的 深度 分 析 也 应 用 于 身体 跟踪 , 它 需 要 较 长 的 处 理 时 间 ,并 且 可 能 无 法 实现 实时 性 。 本 
节 中 ,Kinect XBOX360 用 于 跟踪 人 体 运 动 。 它 的 深度 传感器 可 以 准确 有效、 实时 地 使 用 。 
由 于 Kinect 传感器 提供 的 是 在 笛 卡 儿 空 间 中 的 数据 ,必须 将 其 转换 为 关节 空间 才能 与 机 器 人 
进行 交互 。 本 节 将 介绍 两 种 方法 来 解决 该 问题 , 即 矢量 方法 和 逆 运 动 学 方法 ,并 对 两 种 方法 进 
行 予 比较 。 

11.5.1 未 操作 系统 

1. 系统 配置 

为 了 说 明 使 用 胶体 运动 跟踪 的 机 器 人 遥 操 作 ,建立 如 下 系统 。 它 由 跟踪 装置 和 机 器 人 组 
成 ,如 图 11. 59 所 示 。 


空气 压缩 机 遇 


央 Baxter 机 器 人 


开发 工作 站 


远程 计算 机 


图 11.59 基于 Kinect 的 遥 操 作 控 制 系统 
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身体 跟踪 由 Kinect 传感器 完成 。Kinect 设备 连接 到 远程 计算 机 ,计算 机 上 安装 相应 的 软 
件 用 于 从 Kinect 传感器 接收 位 置 数 据 。 

2. 系统 原理 

遥 操 作 系 统 的 工作 原理 如 图 11. 60 所 示 。 


AR 了 


向 量 方法 | 逆 运 动 学 | 、， 
~ 1 广 ~、 
EL 
> [LinwwUnbunty 上 > 


in ] 


图 11.60 基于 Kinect 的 遥 操 作 系 统 的 工作 原理 


3。Kinect 传感器 

遥 操 作 系统 中 使 用 Kinect V1 传感器 ,具体 介绍 可 参考 10. 3. 3 节 的 相应 内 容 。 在 迁 操 作 
系统 中 ,Kinect 将 人 体 关 节 位 置 和 速度 映射 到 机 器 人 ,使 人 -机 能 够 顺利 地 进行 交互 。 

4. Kinect 开发 软件 

许多 软件 可 用 于 将 Kinect 与 PC 连接 ,例如 , OpenKinect 的 Libfreenect、OpenNI、 
WindowsSDK 的 Microsoft Kinect。 软 件 的 选择 非常 重要 ,选择 软件 时 应 考虑 以 下 几 点 : 提取 
骨架 数据 的 能 力 ; 与 Windows 和 Linux 等 多 种 平台 的 兼容 性 ; 良好 的 说 明文 档 ; 对 算法 验证 
的 快速 性 。 经 比较 ,这 里 采用 满足 以 上 要 求 的 Processing 软件 。 它 可 以 与 Kinect、OpenNI 和 
NITE 的 Sim-pleOpenNI 包 装 器 连接 ,可 以 提取 和 使 用 骨架 数据 ,并且 支持 Windows 和 Linux 

台 , 它 有 很 好 的 说 明文 档 , 验 证 测试 算法 可 以 很 简单 .快速 有效 。 本 节 所 使 用 的 Processing 
中 的 有 用 功能 详 述 如 下 。 

(1) PVector: 描述 为 二 维 或 三 维 向 量 的 类 ,特别 是 欧 几 里 得 (也 称 为 几何 ) 向 量 。 向 量 是 . 
具有 幅度 和 方向 的 实体 。 但 是 ,数据 类 型 存储 向 量 的 分 量 ( 对 于 2D 为 xy, 对 于 3D 为 x、y、 
z) 。 幅 度 和 方向 可 以 通过 方法 mag() 和 heading() 访 问 。 更 详细 的 信息 请 从 网 络 上 查阅 相关 
资料 。 

(2) pushMatrix() 和 popMatrix(): 将 当前 转换 和 矩阵 推送 到 和 矩阵 堆栈 。pushMatrix() 函 
数 将 当前 坐标 系 保存 到 堆栈 , popMatrix() 恢 复 先前 的 坐标 系 。pushMatrix() 函数 和 
popMatrix() 函数 与 其 他 转换 函数 结合 使 用 ,可 以 嵌入 以 控制 转换 的 范围 。 

5. 机 器 人 操作 系统 ROS 

机 器 人 操作 系统 (ROS) 是 一 种 用 于 编写 机 器 人 软件 的 灵活 框架 。 它 是 一 系列 工具 、 库 和 
约定 , 旨 在 简化 在 各 种 机 器 人 平台 上 创建 复杂 而 鲁 棒 的 机 器 人 行为 的 任务 。 

6. Rospy 

Rospy 是 ROS 的 纯 Python 客户 端 库 。Rospy 客户 端 API 使 Python 程序 员 能 够 与 ROS 
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主题 .服务 和 参数 快速 进行 交互 。Rospy 的 设计 有 利于 实现 开发 ( 即 节约 开发 人 员 时 间 ) 而 不 
是 运行 时 性 能 ,因此 算法 可 以 在 ROS 中 快速 进行 原型 设计 和 测试 。 它 也 非常 适用 于 非 关 键 路 
径 代 码 ,例如 配置 和 初始 化 代码 。 许 多 ROS 工具 都 是 用 Rospy 编写 的 ,以 利用 类 型 内 省 功 
能 。 许 多 ROS 工具 ,如 rostopic 和 rosservice, 都 建立 在 Rospy 之 上 。 

7. UDP 协议 

用 户 数 据 报 协议 (UDP) 是 Internet 协议 套件 (用 于 Internet 的 网 络 协 议 集 ) 的 核心 成 员 之 
一 。 使 用 UDP 后 ,计算 机 应 用 程序 可 以 将 消息 (在 本 例 中 称 为 数据 报 ) 发 送 到 Internet 协议 
(IP) 网 络 上 的 其 他 主机 ,而 无 须 事先 通信 来 设置 特殊 传输 通道 或 数据 路 径 。UDP 适用 于 错误 
检查 ,在 应 用 程序 中 不 需要 或 不 执行 校正 ,从 而 避免 了 在 网 络 接口 级 别 进行 此 类 处 理 的 开销 。 
“时间 敏感 的 应 用 程序 通常 使 用 UDP, 因 为 在 实际 系统 中 ,丢弃 数据 包 比 等 待 延 迟 数 据 包 更 
可 取 。 
8. Baxter 机 器 人 

此 处 使 用 的 实验 平台 是 Baxter 机 器 人 。ROS( 机 器 人 操作 系统 )SDK 用 于 控制 Baxter 机 
器 人 和 Baxter 机 器 人 编程 。 


11.5.2 未 操 作 控 制 系 统 设计 
1. 距离 计算 


大 多 数 运 动 计 算 基 于 两 个 或 多 个 位 置 .距离 和 关节 角度 来 计算 二 维 及 三 维 空间 中 两 点 之 
间 的 距离 ,参见 式 (11. 59) 和 式 (11. 60): 


dy =M(zs —zx1) + (ys —y1) (Tl 59) 
di 一 /zy 一 站 十 (yy 一 六 十 (zy 一 < (11. 60) 


其 中 ,(zxj ,yi1) 和 (xz,,y,) 是 2D 空间 中 的 点 ,ds 是 这 两 个 点 之 间 的 距离 ; (ziyyi,zl) 和 (zy， 
ys，z2) 是 3D 空间 中 的 点 ,ds 是 这 两 点 之 间 的 距离 。 余 弦 定 律 可 以 帮助 计算 关节 之 间 的 角 
度 ,最 大 可 计算 角度 为 180"。 在 计算 关节 之 间 的 角度 时 ,需要 额外 的 点 来 确定 180 一 360 范围 
中 具体 的 角度 。 可 以 使 用 骨架 跟踪 数据 中 任意 两 个 关 


、 CC 

节点 绘制 三 角形 。 三 角形 的 第 三 个 点 从 另外 两 个 点 推 。 。 余 纺 定 理 
出 ,如 果 已 知 三 角形 的 每 个 点 的 坐标 ,就 可 以 求 出 每 一 i c s 
边 的 长 度 ,但 无 法 知道 每 个 角 的 角度 。 如 图 11. 61 所 iatbe) 
示 , 可 以 使 用 余弦 定律 来 计算 所 需 角 度 。 关 节点 的 计算 aa 区 洒 
给 出 了 边 a.b.c 的 长 度 。 三 角形 的 角度 可 以 用 余弦 定 
律 计 算 。 图 11.61 余弦 定律 

2. 矢量 方法 


Kinect 可 以 检测 人 体 关节 坐标 并 返回 其 位 置 坐标 。 它 可 以 将 这 些 坐 标 转换 为 矢量 ,并 计 
算 对 应 的 关节 角度 。 在 该 方法 中 ,从 Kinect 提取 身体 关节 的 笛 卡 儿 坐 标 ,并 计算 来 自 肢体 的 
相应 角度 。 然 后 根据 我 们 的 要 求 将 它们 映射 到 与 Baxter 交互 的 Python 代码 中 。4 个 角度 包 
括 肩 部 俯仰 角 、 肩 部 偏转 角 、 肩 部 滚动 角 和 肝 部 俯仰 角 , 如 图 11. 62 所 示 , 根 据 从 Kinect 获得 
的 肢体 位 置 进行 坐标 计算 。 

使 用 矢量 法 的 角度 计算 原理 如 图 11. 63 所 示 。 粗 线 CO 和 CD 分 别 代表 人 的 左上 臂 和 左 
下 臂 。 粗 线 BO 是 从 左 辟 到 左肩 的 线 ,AO 是 从 右 肩 到 左肩 的 线 。 有 向 段 BX 十 .BY 十 和 BZ 十 
表示 Kinect 的 笛 卡 儿 空 间 中 的 帧 的 轴 ,并 且 点 B 是 帧 的 原点 。 

肩 部 俯仰 角 和 肘 部 俯仰 角 的 计算 ,如 图 11. 63 所 示 , 肩 部 俯仰 角 ( 二 BOC ) 由 两 个 矢量 OB 
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冉 部 俯仰 角 


户 部 俯仰 角 
肩 部 滚动 角 


图 11.62 矢量 方法 中 使 用 的 4 个 角度 


和 OC 之 间 的 角度 计算 。 计 算 可 以 通过 使 用 三 个 关节 的 位 置 来 解决 , 即 臂 部 (B 点 ), 肩 部 (O 
点 ) 和 肘 部 (C 点 )。angleOf() 函 数 , 返 回 角 度 可 以 直接 发 送 到 Baxter。 肘 部 俯仰 角 (OCD ， 
OC 和 CD 之 间 的 角度 ) 可 以 通过 将 手 . 有 时 和 肩 点 传递 到 angleOf() 函数 来 计算 。 实 际 上 ,处 理 
软件 中 两 个 矢量 之 间 的 任何 角度 都 可 以 通过 此 方法 使 用 angleOf() 函 数 来 计算 。 

肩 部 偏转 角 的 计算 : 如 图 11. 63 所 示 ,通过 使 用 肩 点 (点 A,O) 和 肘 点 (点 C) 以 相似 的 方 
式 计 算 肩 偏转 角 (EBF) ,构成 向 量 OC 和 O4 。 但 是 这 里 将 这 两 个 矢量 (OC 和 O4 ) 投 影 到 
XZ 平面 以 获得 矢量 BE 和 BF。 然 后 使 用 angleOf() 函数 计算 肩 部 人 往 转角 (FEBF ,BE 和 BF 


之 间 的 角度 ) 。 
ht a 和 BOC: 肩 部 俯仰 角 
| LEBF: 肩 部 偏转 角 
Hh 人 MOH: 房 部 深 动 角 
.| 2 LOCD: 有 时 部 俯 仰角 


图 11.63 矢量 法 中 角度 计算 的 原理 


肩 部 滚动 角 的 计算 : 在 角度 计算 中 , 肩 部 的 角度 计算 是 最 难 的 。 由 于 计算 不 直观 且 所 有 
获得 的 点 都 在 3D 平面 中 ,因此 先前 用 于 计算 角度 的 方法 不 可 用 。 

we 
成 的 角度 。 参 考 矢 量 必 须 相 对 于 身体 稳定 。 因 此 ,通过 采用 肩 肩 矢 量 和 肩 肘 矢量 之 间 的 交叉 
积 来 计算 该 参考 矢量 。 

在 这 种 情况 下 ,可 以 使 用 来 自 两 个 向 量 的 又 积 的 法 线 。 首 先 ,可 以 通过 计算 向 量 OC 和 
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OA 的 又 积 来 获得 向 量 OM 。 矢 量 OM 垂直 于 由 矢量 OC 和 O04 决定 的 平面 ,是 该 平面 的 法 线 
矢量 。 显 然 , 矢 量 OM 垂直 于 矢量 OC( 左 上臂 )。 以 这 种 方式 ,法 线 矢 量 CG 可 以 从 矢量 OC 
和 CD 的 又 积 计算 ,其 也 垂直 于 矢量 OC。 然 后 ,将 矢量 CG 沿 矢量 CO 平移 到 点 O 可 以 得 到 矢 
量 OH。 矢 量 OH 和 OM ,人 MOH 之 间 的 角度 是 肩 部 滚动 角 。 
可 以 使 用 Processing 软件 中 的 PMatrix3D 实例 提取 Kinect 发 送 的 方向 数据 。 方 向 矩阵 
被 赋予 PMatrix3D 变量 。 将 当前 坐标 系 推送 并 保存 到 堆栈 中 。 然 后 将 坐标 系 移动 到 肩 关 节 ， 
并 且 使 用 方向 矩阵 来 表示 变换 的 坐标 轴 。 此 函数 中 的 所 有 计算 都 将 在 此 变换 坐标 系 中 进行 。 
在 计算 滚动 角 之 后 ,通过 从 堆栈 取出 矩阵 来 重新 获得 原始 坐标 系 。 右 肩 滚动 角 也 以 类 似 
的 方式 计算 ,必须 在 向 量 的 方向 上 进行 小 的 改变 。 
因为 用 于 找到 滚动 角 的 功能 并 不 完全 准确 ,所 以 对 肩 部 滚动 角 进 行 了 误差 校正 。 这 里 观 
察 到 的 肩 部 滚动 角 随 肩 部 偏转 而 变化 。 因 此 , 当 肩 部 滚动 角 和 肩 部 偏转 角 的 数据 被 发 送 到 
MATLAB 并 绘制 出 如 图 11. 64 所 示 的 关系 时 ,基于 试 错 法 ,通过 下 式 来 校正 误差 : 
左肩 横 滚 角 = 一 左肩 横 滚 角 一 左肩 偏转 角 /2 一 0.6 (11. 61) 


一 右 肩 偏转 角 


弧度 改变 量 


0 500 1000 1500 2000 2500 3000 
时 间 
图 11.64 矢量 逼近 的 误差 


3. 逆 运 动 学 

逆 运 动 学 是 指使 用 运动 学 方程 从 末端 执行 器 坐标 位 置 计 算 关 节 和 角度 以 及 诸如 机 器 人 的 长 
度 .角度 之 类 的 约束 。Kinect 给 出 了 手 的 坐标 位 置 ,这 些 坐 标 位 置 可 以 转换 为 机 械 臂 的 关节 角度 。 

提取 坐标 : 首先 提取 手 关 节 的 坐标 ,用 于 通过 反 向 运动 控制 最 终 效 果 ; 然后 提取 肘 坐 标 。 
这 些 坐 标 和 手 坐 标 可 用 于 计算 关节 的 长 度 。 

人 和 手 与 Baxter 机 械 臂 之 间 的 映射 , 人手 与 Baxter 机 械 臂 的 大 小 不 同 。 因 此 ,必须 使 用 
Baxter 机 械 臂 对 人 手 进行 匹配 ,以 使 逆 运 动 学 方法 正常 工作 。 如 前 所 述 ,通过 计算 手 - 肘 矢量 
的 模 可 以 得 到 手 的 长 度 。 

分 析 Baxter 坐标 系 : Baxter 坐标 系 的 约定 (如 图 11. 65 和 图 11. 66 所 示 ) 与 Kinect 坐标 
系 的 约定 不 同 ( 如 图 11. 67 所 示 )。 因 此 必须 根据 各 自 的 约定 映射 坐标 轴 。 映 射 后 ,就 可 以 控 
制 Baxter。、 坐 标 轴 之 间 的 映射 关系 如 下 : 

we 二 村 和 Re 
二 Bote — < ine (11. 62) 
z par = ita 
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前 向 视图 2 
103“(260cm) 


注意 : 这 是 夹具 平面 之 间 的 距离 。 夹具 和 手指 将 增加 尺寸 。 
13"” (186cm) 


| 


| 下 


37"(94cm) 躯 干 


70”(178cm) 低 基 座 或 者 73”(185cm) 高 基 座 


| 只 一 


32.0"(55cm) 
> 


图 11.65 ”人手 与 Baxter 机 器 人 的 坐标 图 : 机 器 人 的 前 视图 


天 一 
20(81 
俯视 图 3 LY 
2 


360"(91cm) 托 架 底座 


图 11. 66 ”人手 与 Baxter 机 器 人 之 间 的 坐标 图 : 机 器 人 的 俯视 图 


11.5.3 ”实验 

1. 矢量 法 实验 结果 

当 每 个 步骤 完成 时 ,可 在 接收 角度 和 所 需 角 度 中 观察 到 较 小 的 误差 。 因 此 ,接收 角度 和 所 
需 角 度 与 Processing 软件 中 的 映射 命令 一 起 映射 。 观 察 到 当 所 需 角度 为 一 0. 6 时 ,接收 角度 
为 一 0. 1; 当 所 需 角度 为 0. 6 时 ,接收 角度 为 一 0.9…… 因 此 ,相应 地 映射 这 些 值 。 类 似 地 , 根 
据 所 需 的 角度 校正 所 有 角度 。 最 后 ,Baxter 按 预期 工作 ,但 通过 使 用 这 种 方法 ,只 控制 了 7 个 
关节 中 的 4 个 。 为 了 控制 剩余 关节 ,需要 手掌 和 手指 的 额外 数据 ,这 是 Kinect 无 法 获得 的 ,可 
以 用 逆 运 动 学 方法 解决 。 
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图 11. 67 人手 与 Baxter 机 械 臂 之 间 的 坐标 图 : 人 体 骨 骼 


2. 逆 运 动 学 法 实验 结果 

Processing 能 够 从 Kinect 中 提取 坐标 值 ,从 鼠标 中 提取 鼠标 单 击 事件 。 网 络 协议 能 够 发 
送 数据 ,工作 站 成 功 接收 Processing 发 送 的 值 。Baxter 对 所 有 已 求解 的 位 置 做 出 响应 ,同时 
避免 了 双手 之 间 的 碰撞 。 夹 具 和 气动 配件 能 够 随 着 鼠标 单 击 响应 。 新 用 户 可 能 会 发 现 操作 
它 很 难 , 因 为 机 器 人 肢体 的 向 前 和 向 后 运动 是 人 类 手 部 运动 的 镜像 ,而 肢体 的 侧 向 运动 则 


不 是 。 
11.6 基于 RBF 神经 网 络 和 波 变量 的 双 璧 协同 遥 操 作 控 制 


本 节 基 于 主 端 TouchX 力 反 馈 操 纵 杆 和 从 端 仿真 Baxter 双 臂 机 器 人 ,针对 双 臂 遥 操 作协 
调 性 问题 ,介绍 一 种 阻抗 控制 算法 。 该 方法 可 以 提高 机 械 臂 的 控制 性 能 ,实现 抓 取 远 端 物体 的 
柔顺 控制 。 

单 臂 机 器 人 遥 操 作 技 术 应 用 虽然 广泛 ,但 与 人 类 相 比 ;机 器 人 在 执行 遥 操 作 任 务 时 的 自主 
性 和 灵活 性 的 差距 依然 存在 。 双 臂 机 器 人 控制 和 人 双手 控制 的 类 似 性 ,能 够 完成 单 臂 机 器 人 
难以 完成 的 任务 ,所 以 双 臂 遥 操 作 控 制 成 为 热门 ,前沿 的 遥 操 作 技术 。 对 于 复杂 的 任务 如 搬 
运 .维修 等 , 单 臂 机 器 人 由 于 自身 的 局 限 性 无 法 完成 ,需要 双 臂 协同 作业 实现 。 

与 单 臂 遥 操 作 机 器 人 相 比 , 双 臂 遥 操 作 机 器 
人 适用 范围 更 广 .协作 能 力 更 强 、 可靠 性 也 更 高 ， 
同时 还 能 与 人 类 合作 完成 更 复杂 的 任务 。 基 于 
TouchX 和 双 臂 Baxter 机 器 人 构建 的 双 臂 遥 操 作 
系统 如 图 11. 68 所 示 , 主 端 采 用 两 个 TouchX 设 
备 , 每 个 TouchX 设备 可 以 通过 6 个 旋转 关节 跟踪 
运动 ,并 通过 3 个 配备 电机 的 关节 提供 力 反 馈 。 
双 臂 Baxter 机 器 人 为 从 端 设 备 , 为 了 抓 取 和 处 理 
物体 ,在 每 只 手臂 的 末端 执行 器 上 都 安装 了 旋转 
夹 持 器 。 通 过 计算 机 传递 位 置 与 力 的 控制 信号 ， = 
两 个 TouchX 设备 分 别 完 成 对 Baxter 机 器 人 双 臂 ”图 11.68 基于 TouchX 和 双 臂 Baxter 机 器 人 
的 轨迹 控制 。 的 双 臂 遥 操 作 系统 
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11.6.1 双 禹 协同 控制 遥 操 作 系 统 建 模 
本 节 使 用 的 主 -从 遥 操 作 系 统 框架 如 图 11. 69 所 示 。 


操作 者 


图 11.69 双 辟 遥 操 作 系 统 框架 


1. 遥 操 作 系统 模型 
对 于 图 11. 69 所 示 的 遥 操 作 系 统 中 的 主 端 操作 设备 和 从 端 双 臂 机 器 人 ,其 末端 执行 器 的 
位 置 和 方向 可 以 通过 它们 的 关节 角度 和 角速度 来 计算 。 基 于 主 端 设备 和 远 端 机 器 人 的 正 同 运 
动 学 ,TouchX 力 设 备 1 和 Baxter 机 器 人 在 任务 空间 与 机 器 人 关节 空间 的 关系 可 表示 如 下 : 
po sy PN he do), (11. 63) 
其 中 ,z,ERS 表示 机 器 人 关节 的 位 置 和 方向 ,g 和 G4 ER 分 别 表示 机 器 人 的 关节 角 和 关节 
角速度 ,N, 表示 机 器 人 自由 度 ,T, 为 连续 函数 ,J;(g;) 为 雅 可 比 和 矩阵 。 
基于 以 下 两 个 主 -从 遥 操 作 系 统 进一步 建 模 。 假 设 1: 机 器 人 的 动力 学 是 非 线 性 不 确定 
的 ,但 运动 学 模型 是 准确 的 。 在 运动 过 程 中 ,机 械 辟 远离 任何 奇异 点 。 假 设 2: 远 端 操作 刚性 
物体 时 ,不 会 受到 力 的 作用 而 变形 。 
由 于 两 个 TouchX 触觉 设备 作为 主 设备 ,它们 分 别 可 以 用 相位 “L” 和 “R” 表 示 。 这 里 采用 
主 控 装 置 TouchX 的 LL 和 RR 分 别 控 制 从 端 双 辟 机 器 人 Baxter 的 左右 臂 。 分 别 考虑 TouchX 
左 臂 和 远 端 操作 系统 中 Baxter 机 器 人 左 臂 的 动力 学 ,如 下 : 
M, Cg) 古人 (gm snl ) dn Gl Co ) = (gq )F, Tml C11. 64) 
Te 十 Mu (gy ) 二 Cy (gu ,Gy ) qs Gs (gy) Ty —Jy (gy 2 Chl 65) 
其 中 ,Mi (qn) ER， ,Ci (qus4u) ER “和 Ga (qu)ER 腿 “分 别 代表 惯性 矩阵 , 科 民 
力 /离心 矩阵 和 重力 矢量 ; z+, E 恨 “是 控制 输入 关节 力矩 ,F 是 人 为 施加 的 力 和 撩 量 ,F,, 是 施 
加 在 左 侧 从 动 末 端 执行 器 上 的 力 矢量,J (qj; ) 是 雅 可 比 和 矩阵 。 下 标 m 和 s 分 别 表示 本 地 主 操 
作 设 备 和 远 端 从 机 器 人 。TouchX 和 Baxter 机 器 人 右 臂 的 动力 学 和 式 (11. 64)、 式 (11. 65) 
类 似 。 
远 端 物体 (如 图 11. 70 所 示 ) 的 动力 学 方程 可 以 表示 为 : 
攻克 Le 
其 中 ,zx。 尺 , ER “ 是 从 端 物体 的 位 置 /方向 和 速度 ,M, (xz)、C。(z ,六 ) .G。 (zx) 分 别 表示 该 物 


体 的 惯性 矩阵 、 科 里 奥 利 力 /离心 力矩 阵 和 重力 矩阵 ,而 FP ER 为 施加 在 物体 上 的 力 ,N。 
表示 物体 的 自由 度 。 则 下， 和 下, 之 间 的 关系 可 以 表示 如 下 : 
F,=Fy+F, 一 一 下 一下， ， F=f;+fo Cl 877 
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其 中 ,F。 是 从 机 械 臂 的 末端 执行 器 施加 在 物体 上 的 接触 力 ， hf fh fo 
f。 用 于 产生 物体 运动 的 外 力 ,f; 代表 内 力 相互 抵消 并 满足 1 A 
fi 十 f, 二 0。 下 标 j 二 Lr 分 别 表示 从 端 机 器 人 的 左 辟 和 右 7 一 “人 
辟 。 具 体 受 力 情 况 如 图 11.70 所 示 。 

结合 动力 学 公式 , 即 式 (11. 92) 和 和 式 (11. 93) ,以 及 主 从 
两 端的 运动 学 方程 即 式 (11. 94) ,可 得 
ry 二 My (gy)4 十 Cy (qy 9y)9y 十 GO (qsj) 一 J3 (qsy)f; 图 11.70 远 端 物体 受 力 情况 分 析 

(11. 68) 

其 中 ,M’ =M, +M’,M,=J3D;M,Jy,C'=C;y +C,,C,=JyD;C,J GO 一 Gy 十 G0， 
-一 JIG DER 是 满足 D, (1) 十 D, (1) 二 Ty 的 目标 载荷 分 布 矩 阵 。 

2. RBF 神经 网 络 

利用 RBF 神经 网 络 (Radial Basis Function Neural Network,RBFNN) 对 机 右 人 的 动力 学 
模型 进行 局 部 泛 化 ,可 以 大 大 提高 机 器 人 的 学 习 速 度 ,避免 局 部 最 小 问题 ,提高 机 器 人 的 跟踪 
精度 ,特别 是 对 于 结构 复杂 、 自 由 度 多 的 机 器 人 。RBFNNs 可 表示 为 : 

| ze, | 


pi =exp(————), i =]1,2,.",n Cl1 69) 
o 


i 


F(z)=W'o9(z) (11. 70) 
式 中 ,z ER” 为 输入 向 量 ,n 为 从 端 机 器 人 的 自由 度 , 对 于 Baxter 机 器 人 左 辟 ,n 为 7, 式 中 ， 
人 (=)ER” 为 输出 向 量 ,g; = 二 [qi1,9s，…,9,] 为 隐藏 层 的 输出 向 量 ,WE RN*" 为 连接 隐藏 
层 和 输出 层 的 权重 矩阵 , N 为 隐藏 层 节 点 数 ,c; E R” 和 二 0 分 别 是 第 i 个 隐藏 节点 的 中 心 
向 量 和 宽度 。 从 式 (11. 68) 开 始 , RBFNN 中 隐藏 节点 的 输出 由 径 向 对 称 函 数 ( 如 高 斯 函数 ) 
计算 。 
本 节 中 ,RBF 神经 网 络 主要 用 于 近似 不 确定 非 线 性 函数 下 (xz)。 
3. 波 变量 法 | 
本 节 使 用 基于 时 变 延 迟 通信 的 波 变量 方法 。 在 通信 信道 中 , 波 变量 之 间 的 时 变 延 迟 TT 
和 了 ,可 以 表示 如 下 : 
u,(t) =u,, (t — T(t)) CLL, 7 
v(t) =vw (tO— T(t EE 
其 中 ,u 和 w 是 由 速度 之 和 力 下 传递 到 通信 通道 中 的 功率 变量 ,这 两 种 变量 之 间 的 关系 如 
图 11.71 所 示 。 
采用 波 校正 方法 处 理 远程 操作 系统 通信 中 的 时 变 延 迟 , 如 图 11. 85 所 示 , 可 以 表示 如 下 : 


Kw (EY) = WN + a rg EY GD 739 
Avn (2) = M2DALT py GT — roma Ct) (11.74) 


其 中 ,过 0 是 为 位 置 收敛 而 设计 的 。 

11.6.2 从 端 控制 器 设计 

主 端 控制 器 由 基本 PD 控制 算法 计算 。 本 节 将 介绍 一 种 基于 标 称 模型 的 力矩 控制 和 一 个 
神经 网 络 控制 器 ,用 于 处 理 不 确定 性 问题 。 

闭环 逆 运 动 学 (CLIK) 方 法 在 从 端 双 辟 机 器 人 控制 时 用 于 避免 运动 奇 点 和 数值 漂移 ,如 
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图 11.71 波形 变量 通信 


图 11.72 所 示 。 从 端 PD 控制 器 设计 如 下 : 
= ED (11.75) 
其 中 ,ev = 二 gq, 一 qa 是 跟踪 误差 ,gu ER” 是 用 作 局 部 PD 控制 器 的 参考 命令 的 期 望 关 节 角 


度 ,K, ER"” ”和 D,ER” ”是 关节 的 对 称 正 定 矩 阵 角度 和 角速度 增益 。 


T 


定义 广义 跟踪 误差 : 
Csj = Ee . 涌 阁 眉 76) 
其 中 天, 一 DJ K， 
rj =—D,e . CTI 7 
定义 gu 一 dd 一 Kyesy， 从 端 机 器 人 的 运动 学 公式 为 
Mjévs + Cev TD =—J37; Gs +M,dy Cv CT 8 


输入 z 的 不 确定 非 线 性 动力 学 公式 可 以 表示 如 下 : 
Fer Y= 0 Md Cy Cl1 ?79) 
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对 于 不 确定 模型 即 式 (11. 79) ,神经 网 络 具 有 强大 的 函数 逼近 能 力 , 可 用 于 不 确定 模型 的 识别 。 
基于 RBFNN 方法 , 式 (11.79) 可 写成 : 


Me =—(C, De 十 人 (zi ,W) )+7 (11. 80) 


式 中 ,w= 二 F(z,) 一 全 (z; ,W;),W’ 为 2; EX 对 应 的 最 优 权 和 矩阵 。 
根据 RBF NN 的 性 质 , 式 (11. 80) 可 以 写成 : 


M ,és = Cs 二 De +W; 9g(z;) 站 用 灿 
则 用 李 雅 普 诺 夫 方 法 可 以 得 到 如 下 更 新 规律 : 
W, =—Q7 pC) en, (11. 82) 


其 中 ,Qi 是 对 称 正定 矩阵 , 且 RBFNN 权重 的 有 界 性 可 以 通过 激励 (PE) 属 性 的 持续 性 来 保证 。 
控制 力矩 由 两 部 分 组 成 ,如 下 : 


ry 一 一 Dew HWIgy(z,) (11. 83) 
则 从 端 机 器 人 闭环 系统 的 动力 学 方程 可 表示 为 : 

Me +Csev 十 De =F(z,) 一 全 (=)) (11. 84) 

在 机 器 人 控制 系统 中 ,机 器 人 和 外 物 有 直接 接触 时 ,需要 一 定 的 和 柔性。 这 里 希望 能 用 控制 

算法 让 机 械 臂 表现 出 期 望 的 柔性 。 最 简单 的 有 

弹性 且 不 会 永久 震荡 的 系统 就 是 质量 -弹簧 - 阻 


尼 系 统 , 即 图 11.73 所 示 的 系统 。 


因此 ,设计 阻抗 控制 器 如 下 : 
V(b) =V,(H,(H,(0),H,(0)),H, ,Rk,)+ 
V,(H,(0),H,(0),k.) (11. 85) 
其 中 ,o 类 似 于 一 个 立体 弹簧 , 互 ,。。 为 虚拟 的 平 
衡 位 置 ,c 是 阻抗 系数 。 图 11.73 质量 -弹簧 -阻尼 系统 
11.6.3 实验 


针对 双 臂 遥 操 作 实验 控制 ,搭建 包括 主 设备 和 从 设备 的 实验 平台 。 主 端 操作 设备 采用 两 个 
触摸 式 操纵 杆 TouchX ,从 端 机 器 人 采用 机 器 人 工具 箱 仿 真 的 Baxter 双 臂 机 器 人 。TpuchX 的 两 
个 操纵 杆 首先 由 操作 员 操 作 ,然后 实时 位 置信 息 分 别 通过 通信 通道 传输 到 仿真 双 臂 机 器 人 的 两 
个 手臂 上 。 控 制 两 个 仿真 机 械 臂 协同 抓 住 一 个 共同 的 物体 。 然 后 ,由 Simulink 工具 测量 的 双 臂 
机 器 人 与 物体 之 间 的 相互 作用 力 , 通 过 通信 反馈 给 主 端 ,最 终 由 操作 者 双手 感知 远程 交互 力 。 脖 
操作 系统 的 结构 如 图 11. 74 所 示 ,实验 平台 如 图 11.75 所 示 。 图 11. 74 使 用 具有 校正 波 改进 的 波 
变量 算法 能 够 保证 双 臂 遥 操 作 系统 的 无 源 性 ,并 在 存在 时 变 时 延 的 情况 下 保证 系统 稳定 性 。 


” 通信 
(时 变 时 延 ) 


图 11.74 基于 全 局 神经 网 络 控制 器 的 波 变量 校正 遥 操 作 系 统 的 结构 
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图 11.75 双 臂 遥 操 作 实 验 平 台 


实验 分 别 对 双 臂 机 器 人 的 跟踪 轨迹 与 力 反 馈 进 行 测试 。 实 验 结果 表明 ,考虑 被 操控 物体 
动力 学 模型 的 阻抗 控制 算法 可 以 保证 跟踪 轨迹 误差 的 收敛 性 ,而 加 入 波 变 量 算法 则 可 以 保证 
遥 操 作 系统 的 稳定 性 。 

1. 轨迹 跟踪 性 能 

仿真 Baxter 机 器 人 双 臂 在 存在 时 变 时 延 的 情况 下 跟踪 轨迹 ,从 端 左 臂 和 右 臂 对 主 端 的 
轨迹 跟随 效果 如 图 11. 76 和 图 11. 77 所 示 。 实 验 结果 表明 ,该 系统 具有 良好 的 轨迹 跟踪 
性 能 。 


时 间 延 迟 /s 


了 方向 /m 


1.0 一 一 主 端 机 器 人 -一 -一 从 端 机 器 人 


了 方向 /m 


一 一 主 端 机 器 人 ----- 从 端 机 器 人 


~ 


Z 方 向 /m 


时 间 /s 
图 11.76 在 通信 端 存 在 时 变 时 延 情况 下 ,使 用 波 变 量 与 RBF 神经 网 络 算法 ， 
从 端 机 器 人 左 臂 轨迹 (虚线 ) 对 主 端 机 器 人 轨迹 ( 实 线 ) 的 跟踪 效果 
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es 


时 间 延 迟 As 


一 一 主 端 机 器 人 …--- 从 端 机 器 人 


jm - ~. 


了 方向 mm 
1 


SE 一 一 主 端 机 器 人 ----- 从 端 机 器 人 


了 方向 /m 


Z 方 向 /m 


时 间 /s 
图 11.77 在 通信 端 存在 时 变 时 延 情况 下 ,使 用 波 变量 与 RBF 神经 网 络 算法 ， 
从 端 机 器 人 右 臂 轨迹 (虚线 ) 对 主 端 机 器 人 轨迹 ( 实 线 ) 的 跟踪 效果 


2. 力 反馈 跟踪 性 能 

实验 中 , 主 端 两 个 触觉 操作 设备 TouchX 接收 来 自 远程 环境 的 交互 力 , 而 由 于 使 用 校正 波 
改进 的 波 变量 算法 ,整个 遥 操 作 系 统 在 时 变 时 延 情 况 下 保持 稳定 。 遥 操作 系统 主 端 左 、 右 侧 的 
力 反馈 分 别 如 图 11. 78 和 图 11. 79 所 示 ,可 以 看 出 遥 操 作 系 统 良 好 的 稳定 性 和 力 反馈 性 能 。 
根据 主 从 两 端 机 器 人 的 动力 学 模型 ,设置 控制 参数 为 : D, = 50N-s/m，, K,; 一 1000N/m， 
K,=100N/m,R, =6=180N-s/m,4=100。 


时 间 延 迟 /s 
© 
| 
N 
| 
) 


时 间 /s 
图 11.78 在 通信 端 存在 时 变 时 延 情况 下 ,使 用 波 变量 与 RBF 神经 网 络 算法 ， 
主 端 机 器 人 左 臂 力 反馈 效果 


326 硬 || 机 器 人 控制 一 一 运动 学 、 控 制 器 设计 、 人 机 交互 与 应 用 实例 


时 间 延 迟 /s 


于 方向 /m 


7 方向 /m 


三 
NE 
全 
| 
| 时 间 /s 
图 11.79 在 通信 端 存在 时 变 时 延 情 况 下 ,使 用 波 变量 与 RBF 神经 网 络 算法 ， 
主 端 机 器 人 右 臂 力 反馈 效果 


从 端 控制 器 双 臂 神经 网 络 训练 过 程 中 权 值 更 新 如 图 11. 80 所 示 。 


站 
时 间 /s 


图 11. 80” 双 辟 神 经 网 络 训 练 过 程 中 权 值 更 新 


对 于 双 臂 机 器 人 来 说 ,精确 地 移动 一 个 共同 的 物体 需要 两 个 手臂 之 间 的 实时 合作 。 在 这 
种 情况 下 ,必须 考虑 包括 施加 在 物体 上 的 内 力 在 内 的 力 的 相互 作用 。 因 此 ,本 节 首 先 分 析 了 主 
设备 .从 设备 和 对 象 的 动力 学 模型 。 由 于 从 端 机 器 人 动力 学 模型 存在 不 确定 性 ,因此 采用 神经 
网 络 方法 对 其 局 部 进行 补偿 。 为 了 保证 遥 操 作 系 统 的 稳定 性 ,通信 部 分 采用 了 波 变量 法 。 然 
后 ,根据 控制 器 的 动力 学 模型 ,分别 设计 了 主 控制 器 和 从 控制 器 。 利 用 李 雅 普 诺 夫 函 数 证 明了 
系统 的 跟踪 收敛 性 和 稳定 性 。 通 过 仿真 实验 ,证 明了 该 方法 具有 展 好 的 轨迹 跟踪 和 力 反馈 
性 能 。 
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11.7 本 章 小 结 


本 节 主 要 介绍 了 机 器 人 控制 技术 在 遥 操 作 方向 的 一 些 具体 应 用 实例 ,这 些 应 用 实例 为 该 
领域 比较 前 沿 的 研究 方向 。 其 中 包括 基于 人 体 表 面 肌 电 信号 、 脑 机 接口 、 视 觉 融合 技术 以 及 触 
觉 信 号 等 对 机 械 辟 进行 遥 操 作 或 者 三 维 遥 操作 控制 ; 还 介绍 了 基于 双 臂 Baxter 机 器 人 对 人 
体 运动 进行 跟踪 和 双 臂 的 协同 控制 。 感 兴趣 的 读者 可 以 结合 自己 的 研究 方向 进行 更 深入 的 
研究 。 


机 兹 人 示 教 


CHAPTER 12 


人 们 越 来 越 期 望 机 器 人 能 够 具有 灵活 的 操作 技能 ,能 够 在 物理 人 机 交互 系统 中 适应 更 复 
杂 的 任务 情况 。 示 教 编程 (Programming by demo,PbD) 技 术 被 认为 是 机 器 人 学 习 人 类 的 运 
动 和 操作 技能 的 最 有 效 方法 之 一 。 传 统 的 PbD 方法 ,例如 操纵 杆 、 键 盘 或 人 体 运 动 捕捉 设备 ， 
更 专注 于 端点 移动 轨迹 规划 和 控制 的 快速 编程 。 这 样 的 接口 对 于 仅 依赖 于 位 置 的 简单 任务 可 
能 是 有 效 的 。 然 而 ,它们 难以 适用 于 复杂 的 交互 ,特别 是 在 物理 交互 这 样 需要 同时 调节 位 置 与 
刚度 的 任务 场景 中 。 

示 教 编程 又 称 从 示 教 中 学 习 (learning from demo,LfD) ,是 一 种 降低 机 器 人 执行 新 任务 
复杂 性 的 有 效 方法 。 通 过 PbD, 机 器 人 能 够 有 效 地 学 习 操 作 技 能 ,通过 人 类 的 指导 完成 任务 ， 
即 人 先 示 教 如 何 执行 一 项 任务 ,然后 机 器 人 学 习 运 动 的 特征 。 与 传统 的 编程 方法 相 比 ,PbD 
具有 许多 优点 ,例如 : 它 并 不 特别 需要 示 教 者 具备 专业 技能 知识 ; PbD 系统 可 以 考虑 人 的 特 
性 (如 灵活 性 ) ,这 在 很 大 程度 上 有 利于 完成 作业 任务 。 

本 节 将 介绍 几 个 关于 机 器 人 示 教 的 具体 应 用 实例 。 


12.1 应 用 于 人 机 示 教 的 运动 技能 扑 捉 .传递 与 拓展 
12.1.1 实验 仿真 平台 介绍 


1. V-REP 

V-REP 是 一 个 基于 分 布 式 控制 框架 的 机 器 人 仿真 软件 , 它 集 成 了 开发 环境 (用 户 界面 如 
图 12. 1 所 示 )。 软 件 中 的 每 个 对 象 或 模型 都 可 以 通过 一 个 可 移植 脚本 .一 个 插件 、 一 个 ROS 
节点 或 者 一 个 远程 API 客户 端 独 立 控制 ,这 使 得 V-REP 成 为 机 器 人 仿真 应 用 的 理想 工具 。 
其 中 ,控制 器 可 以 通过 多 种 语言 进行 编写 ,包括 C/C++、Lua、Matlab、Python 以 及 Octave。- 
V-REP 可 以 用 于 算法 快速 开发 .工厂 自动 化 仿真 .原型 的 快速 制作 和 验证 、 机 器 人 教育 等 相关 
领域 。 

2. Baxter 机 器 人 

Baxter 机 器 人 手腕 处 装 有 摄像 头 , 每 个 关节 都 有 高 分 辩 率 的 力度 传感器 ,其 内 置 的 传 感 
器 可 以 测量 Baxter 机 器 人 的 位 置 .速度 ,状态 信息 。 其 编程 开发 接口 是 建立 在 ROS 之 上 ,这 
使 得 编程 接口 的 调用 更 加 简单 高 效 。 

3. Kinect 传感器 

Kinect 传感器 在 实验 中 作为 双 目 摄像 头 , 用 于 捕捉 示 教 者 的 手臂 关节 信息 动作 以 及 手 
臂 的 运动 轨迹 。 
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图 12.1 V-REP 用 户 界 面 


12.1.2 人 机 示 教 系统 设计 

1. 人 机 示 教 系统 设计 

本 节 将 设计 一 个 比较 完整 的 人 机 示 教 系统 ,包括 技能 传递 .拓展 以 及 对 拓展 运动 进行 准确 
轨迹 跟踪 的 控制 器 ,其 整体 设计 框架 如 图 12. 2 所 示 。 


技能 拓展 相关 参数 


图 12.2 人 机 示 教 系统 整体 设计 框架 


示 教 者 首先 要 对 机 器 人 进行 示 教 ,完成 技能 传递 模型 的 训练 学 习 。 根 据 所 使 用 传感器 的 
不 同 , 示 教 方式 也 会 有 所 不 同 ,如 基于 遥 操 作 ,或 者 直接 引导 机 器 人 完成 示 教 等 。 

当 技 能 学 习 模型 训练 完成 后 ,就 可 以 通过 模型 获得 传递 及 拓展 后 的 运动 轨迹 。 通 过 调整 
模型 的 相关 参数 (包括 运动 轨迹 的 位 置 参 数 和 时 间 参 数 ) ,可 以 获得 新 的 运动 轨迹 , 即 实现 了 时 
间 上 与 空间 上 的 拓展 。 该 模型 可 以 应 用 于 笛 卡 儿 空间 或 关节 空间 中 的 运动 轨迹 ,本 节 主 要 以 
关节 空间 为 例 ,对 相关 模型 算法 进行 介绍 。 

对 于 模型 生成 的 运动 轨迹 ,还 需要 考虑 轨迹 的 跟踪 精度 问题 。 以 关节 空间 的 轨迹 为 例 ,用 
一 个 PD 控制 器 控制 机 械 臂 的 力矩 。 考 虑 到 存在 不 确定 负载 ,假设 控制 器 中 还 应 包含 前 馈 补 
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偿 部 分 ,以 补偿 未 知 负载 对 控制 效果 的 影响 。 本 节 将 介绍 使 用 神经 网 络 通 近 机 械 臂 动力 学 模 
型 中 的 未 知 函 数 。 由 于 其 中 的 技能 传递 模型 使 用 动力 学 方程 表示 ,因此 整个 系统 可 以 构成 一 
个 完整 的 闭环 系统 。 弹 簧 阻尼 系统 自身 对 于 扰 具 有 一 定 的 鲁 棒 性 ,可 与 轨迹 跟踪 控制 器 相 结 
合 , 使 机 器 人 更 加 精准 地 完成 整个 运动 过 程 。 
2. 动态 运动 原 语 模型 
动态 运动 原 语 (Dynamic Motor Primitive, DMP) 可 以 分 为 离散 DMP 和 周期 DMP, 分 别 
用 来 表示 点 到 点 的 运动 和 周期 型 运动 。DMP 可 以 用 于 表示 笛 卡 儿 空 间或 关节 空间 上 的 运动 
轨迹 。 本 节 将 使 用 离散 DMP 来 表示 关节 空间 上 的 轨迹 , 且 每 一 个 关节 的 状态 使 用 一 个 DMP 
模型 来 表示 。 
一 个 DMP 模型 包含 了 一 个 弹簧 -阻尼 系统 和 一 个 外 力 项 ( 非 线性 项 ) 。 其 模型 定义 如 下 : 
roO 一 RE 一 0) 一 cw 十 (一 00)s 太 (Cs) 
et 1 
w=w 
其 中 ,09ER 为 关节 位 置 ,w ER 为 关节 速度 ,多 /rER 为 关节 加 速度 ,g ER 为 目标 位 置 ,9, ER 
为 起 始 位 置 ,(g 一 0, ) 为 空间 缩放 项 ,rt0 为 时 间 缩 放 因 子 ,& 二 0 和 cc 二 0 为 模型 参数 。 
假设 /: RR 是 一 个 非 线性 的 连续 有 界 函 数 ,* 之 0 为 一 阶 动力 学 系统 的 状态 ,其 模型 
如 下 : 
TS ==—a,s (12. 2) 
其 中 ,a., 二 0 为 时 间 常 数 。 该 系统 的 引入 使 得 模型 的 非 线性 部 分 独立 于 时 间 ,进而 使 整个 模型 
成 为 一 个 自治 的 系统 。 状 态 * 被 看 作 一 个 相 变 量 ,通常 选取 其 初 值 为 so 二 1。s 是 单调 递减 
的 ,最 终 会 收敛 到 0。 
原始 的 DMP 中 ,其 非 线 性 函数 f(s ) 预 先 定义 为 如 下 形式 : 


N 
f(s) 一 >)7igi (Cs) (12. 3) 


其 中 ,y ER 为 ,5) 的 权重 ,$8,(s) 为 归 一 化 的 径 向 基 丽 数 ,其 形式 如 下 。 


exp(—h;(s—c;)’) 
pis) (12. 4) 


友和 人 一 也 


j=1 

其 中 ,c ,二 0 为 高 斯 基 函 数 的 中 心 ,h ;二 0 为 其 宽度 ,NN 为 高 斯 函数 的 数量 。 

DMP 具有 如 下 特性 。 

(1) 稳定 性 和 和 鲁 棒 性 。 假 设 fs) 王 0, 系 统 为 一 个 简单 的 弹簧 阻尼 系统 。 当 c 之 0 且 二 0 
时 ,该 系统 是 稳定 的 。 一 般 情况 下 ,选取 k=c*/4, 使 弹簧 阻尼 系统 处 于 临界 阻尼 状态 ,以 使 其 “ 
状态 尽 可 能 快 地 到 达 目 标 位 置 g ,如 图 12. 3(a) 所 示 。 由 于 函数 f(s) 是 有 界 的 , 且 s 最 终 会 收 
敛 到 0, 如 图 12.3(b) 所 示 , 因 此 非 线 性 项 最 终 会 收敛 到 0, 整个 系统 最 终 会 稳定 在 目标 位 置 ， 
即 保证 机 器 人 运动 到 目标 位 置 。 此 外 ,弹簧 阻尼 系统 也 保证 了 该 模型 对 外 部 扰动 的 鲁 棒 性 。 

(2) 空间 缩放 和 时 间 缩 放 。 通 过 修改 DMP 的 参数 g 和 0, ,可 以 得 到 一 个 与 示 教 轨迹 形 
状 相 似 但 收敛 到 不 同位 置 的 轨迹 。 通 过 设置 时 间 因 子 =, 可 以 调整 运动 速度 。 

DMP 学 习 问 题 的 本 质 在 于 如 何 学 习 函 数 f(s), 也 就 是 确定 高 斯 函数 的 权重 ,可 以 通过 使 
用 局 部 加 权 回 归 算 法 解决 。 然 而 这 种 方法 只 能 用 于 学 习 单个 示 教 轨迹 的 数据 。 为 了 给 多 次 示 
教 轨迹 的 数据 建 模 , 这 里 引入 高 斯 混合 模型 (Gaussian mixture model,GMM) 。 

3. 动力 学 模型 拓展 


对 于 给 定 的 示 教 轨迹 {0,, ,0，, ,5,,，} /1, 其 中 9, ER 为 关节 位 置 ,T, 为 示 教 时 间 
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1.00 
0.75 
~ 
和 0.50 
0.25 
0 25 50 75 100 
时 间 /s . 
(a) 弹簧 阻尼 系统 状态 演化 
1.00 
0.75 
去 0.50 
0.25 
0 25 50 75 100 
时 间 /s 
(b) 典型 系统 状态 演化 
图 12.3 DMP 各 部 分 系统 状态 演化 
长 度 ,N 为 示 教 轨迹 的 数量 。 首 先 ， 利用 这 些 数据 计算 出 数据 集 4 Ts of pe i 


(0,1] 为 系统 在 t 时 刻 的 状态 ,了 ,,, ER 则 是 通过 将 s,、90,,,、9,,,、9,,, 代入 式 (12. 1) 计 算得 到 
(如 图 12.4(a)、(b) 所 示 )。 当 N= 二 1 时 ,也 就 是 只 有 单个 示 教 轨迹 数据 时 ,函数 f(s) 可 以 使 用 
局 部 线性 回归 从 数据 集 {s, , f, 1 ) 全 。 中 学 习 得 到 。 然 而 这 个 方法 并 不 适用 于 同时 学 习 多 个 示 
教 轨迹 数据 。 为 了 解决 这 个 问题 ,使 用 高 斯 混合 模型 来 为 这 些 数据 建 模 。 为 了 表达 方便 ,后 述 
将 使 用 {s ,了 } 来 表示 数据 集 {5, ,f 1,) 全 六 1。 

高 斯 混合 模型 是 一 种 由 一 组 高 斯 分 布 组 成 的 具有 未 知 参数 的 概率 模型 。 高 斯 混合 模型 是 
一 种 统计 学 方法 , 它 常 用 于 聚 类 或 者 密度 估计 。 将 其 与 高 斯 混合 回归 算法 相 结 合 , 可 用 于 非 线 
性 函数 的 估计 。 本 节 使 用 高 斯 混合 模型 表示 s ,了 的 联合 概率 P(s,f) ,其 定义 如 下 :， 


K 
P(s,f)= Da Nss Fs Hk yn 
k=1 


K 
a 
和 

3 P bs > (12. 5) 
Le 2 9 == 
: Lf,k Zp Zp 


—0.5C[s,f] —pn) Br (Cs,f] op,) 


re Ss 二 
其 中 ,K 为 高 斯 模型 的 个 数 ,a, 过 0 为 先 验 概率 ,yj E pd 为 第 & 个 高 斯 模型 的 均值 ,又 和 
R*” 为 第 & 个 高 斯 模型 的 协 方差 矩阵 ,NGs ,了 ; py ,3 ) 为 高 斯 概率 分 布 。ak xs、 是 未 知 
的 模型 参数 ,可 以 通过 期 望 最 大 化 (Expectation Maximization ,EM) 算 法 学 习 得 到 。 该 算法 是 
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最 大 似 然 估计 的 一 种 迭代 计算 方法 ,其 对 参数 的 初 值 十 分 敏感 。 一 般 可 以 使 用 均值 (R- 
means) 算 法 初始 化 这 些 参 数 。 此 外 ,高 斯 模型 的 数量 也 影响 着 估计 的 误差 和 平滑 程度 ,其 数 
值 可 以 依靠 经 验 选取 ,也 可 以 利用 贝 叶 斯 信息 度量 方法 进行 估计 得 到 。 

当 从 数据 集 {s ,了 } 中 学 习 完 高 斯 混合 模型 后 (如 图 12. 4(c) 所 示 )， 就 可 以 通过 高 斯 混合 回归 


算法 得 到 非 线 性 函数 的 估计 值 A。 在 高 斯 混合 回归 中 ,对 于 给 定 值 ; ,了 的 条 件 概率 定义 如 下 : 
K 

PF SY DBM ode 
k=1 


从 Hf,k st Sp,p (人 iT eg 


436 Tm Zp (By i (12. 6) 
Ns; Ks 下 » 

Bi = < 
> NG. we s,s 


根据 高 斯 分 布 的 线性 变换 特性 ， PCF15) 可 以 近似 为 
全 | 5) 一 NGg,62) 


s 到 
?一 之 久久 本 的 
K 
0’ = 2810t 
k=1 
其 中 ,用 为 期 望 值 , 其 值 作为 f(s) 的 估计 值 (如 图 12. 4(d) 所 示 ): 
f(s) = p> J (12. 8) 
2000 
1500 
~ 1000 
S00 
002 0.4 0.6 0.8 1.0 
时 间 :有 
(a) 展示 (b) 数据 集 fs, 用 
2000 2000 
1500 1500 
1000 1000 
500 500 
0 0 


02 O04 QE 08 :0 02 -04” 06 08， 10 
S S 
(c) 学 习 后 的 GMMs (d) GMR 结 果 


图 12.4 GMM/GMR 学 习 过 程 
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通过 高 斯 混合 回归 估计 得 到 的 非 线性 函数 在 形式 上 包括 B 和 炙 这 两 项 ,其 中 Bi /a 为 归 一 
化 的 高 斯 函数 。 此 处 估计 得 到 的 非 线性 函数 多 乘 了 尔 ==p yi 十 Bp.4 (ED) 1!(s 一 p64) 项 ,这 
意味 着 它 包 含 了 更 多 的 表征 数据 的 特征 值 ,因此 可 以 使 DMP 从 多 次 示 教 轨迹 中 获取 更 多 的 
运动 技能 信息 。 
12.1.3 神经 网 络 控制 器 设计 
1. 控制 器 设计 
本 小 节 将 设计 一 个 基于 神经 网 络 的 机 械 辟 控制 器 ,以 实现 对 期 望 关节 轨迹 的 跟踪 ,该 控制 
器 可 以 克服 不 确定 负载 对 机 械 臂 控制 效果 的 影响 。 以 7 自由 度 的 Baxter 机 器 人 为 控制 对 象 ， 
在 外 加 负载 的 情况 下 ,其 动力 学 模型 描述 如 下 : 
M(g)s FC ,04 G(r =r (12. 9) 
其 中 ,g ER" 为 机 械 臂 的 关节 位 置 ,M(Co)ER" .Clg,9)ER”” 和 G(Co)ER 分 别 为 机 械 
臂 的 惯性 矩阵 、 科 里 奥 利 和 矩阵 和 重力 项 。resx 为 施加 负载 造成 的 外 部 力矩 。 定 义 es 二 gq 一 qa， 
5 一 6 十 Ae ,v= 二 94 一 Aey ,其 中 人 二 diag(X1,42，…,A47) ;二 0,qy 为 参考 轨迹 。 式 (12. 9) 可 
以 推导 为 : 
M(g);+C(g,9)s +G(g) +MY +C(g, 9) =r — to (12. 10) 
系统 的 控制 力矩 设计 如 下 : 
rz 一 和 G 十 Ma 十 Co 十 ru 一 区 s Chr Lly 
其 中 ,G、WM 和 CC 分别 是 GCq) .MGo) 和 Co,o) 的 估计 矩阵 ,KER 为 对 角 和 矩阵 。 此 时 系 
统 的 闭环 动力 学 方程 如 下 : 
M(o)i 二 CCo,o)s 十 Ks 一 一 (OM 一 M)D 一 (C 一 C)o 一 (G 一 6) Cl) 
使 用 神经 网 络 分 别 通 近 M(g)、C(g,9) 和 G(g), 有 : 
EE =WhMZm (9) 
Cl(g,9) =WLZc (gq ,9) CE 1 
CH = WZ 
其 中 ,Wy ER W. ER WER "为 权重 矩阵 ,Zum (gq)、Zc (lq,9) 和 Zo(g) 为 径 向 
基 函 数 矩 阵 , 为 神经 网 络 节 点 数 。M(g)、Cl(g,9) 和 G(g) 的 估计 值 可 以 写 为 : 
M(g) =WHIZu Cd) 
Ee =WiZc (gqg,0) (12. 14) 
Gq) =WLZo (gq) 
将 上 式 代 入 式 (12. 12) ,可 以 得 到 ， 
M(g);+C(g,9)s + Ks =—WH Zu — WLZcv — WLZe 《说 : 15) 


WE 一 WI 一 全 (12. 16) 


2. 稳定 性 证 明 
选取 如 下 李 雅 普 诺 夫 函数 : 
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VY = zs Ms 十 Ftr WHQuWy +WiQcWe +WIQoWoe) (1 
其 中 ,Qw .Qc 和 Qe 为 正定 权重 矩阵 。V 的 一 阶 导 数 为 : 
V=—s'Ks —itr[Wh (Zuds! + QuWu )]—ir[LWE (Zevs! + QeWe 和 
tr [WE (ZosT + QW )] Ch 8 
神经 网 络 权 重 的 更 新 律 设计 如 下 : 
. ~ =—QMZmds! 


We =—QaZevsT (12. 19) 


W, =—QaZ6sT 
将 式 (12.19) 代 入 式 (12. 18) ,可 得 ; 

V=—si'Ks 及 0 C1 OS 
易 证 明 ,s 最 终 将 收敛 到 0, 进而 有 e, 一 ,0 系统 输出 将 跟踪 从 DMP 模型 学 习 好 的 参考 轨迹 。 


12.1.4 仿真 及 实验 

1. V-REP 仿真 平台 下 的 DMP 仿真 验证 

1) 仿真 平台 搭建 

仿真 实验 中 ,使 用 V-REP 自 带 的 Baxter 仿真 机 器 人 模型 ,如 图 12.5 所 示 。 

首先 使 用 Kinect 捕捉 示 教 者 的 手臂 运动 过 程 , 然 
后 将 示 教 信息 ( 即 手臂 关节 和 角度) 传递 给 虚拟 Baxter 
机 需 人 的 手臂 。 在 该 仿真 实验 中 ,机 器 人 在 关节 空间 
上 学 习 人 的 示 教 动作 使 用 了 示 教 者 两 个 肩 关 节 和 一 
个 肘 关节 的 信息 数据 进行 实验 ,如 图 -12.6(a) 所 示 。 
仿真 场景 设置 如 图 12.6(b) 所 示 。 在 该 场景 中 ， 
Baxter 机 器 人 的 前 方 摆 放 着 一 张 桌 子 。 桌 子 靠近 机 
器 人 的 边缘 处 放置 两 块 彼 此 相 邻 的 红色 木板 ,其 中 
一 块 作为 示 教 动作 的 终点 标识 , 另 一 块 则 作为 拓展 图 12.5 VREP 中 的 Baxter 仿真 机 器 人 
动作 的 终点 标识 。 此 外 ,在 Baxter 机 器 人 左手 前 方 
摆 放 一 个 长 圆柱 体 , 圆 柱 顶 端 摆 放 有 蓝 色 小 球 , 用 来 检查 是 否 发 生 了 碰撞 。 在 本 次 仿真 实验 
中 ,机 器 人 的 任务 是 使 用 学 习 到 的 运动 技能 ,将 桌子 上 摆 放 的 木板 推倒 在 地 。 如 果 Baxter 机 
器 人 使 用 其 左手 臂 来 完成 这 项 任务 , 它 就 必须 避 开 左手 臂 前 方 的 障碍 物 。 示 教 者 将 会 向 机 器 
人 展示 如 何 绕 开 障 碍 物 以 完成 这 个 任务 。DMP 模型 用 于 上 述 人 类 运动 技能 的 学 习 , 并 进一步 
对 技能 进行 拓展 。 

2) 基于 DMP 的 运动 技能 传递 仿真 

示 教 过 程 如 图 12.7 所 示 ,机 器 人 学 习 将 左边 (相对 于 机 器 人 ) 的 木 块 推翻 的 运动 技能 ,并 
通过 DMP 模型 重 现 该 运动 。Baxter 机 器 人 模仿 示 教 者 的 运动 动作 以 完成 任务 ,其 运动 过 程 
包括 : 机 器 人 将 左手 臂 抬 起 并 跨 过 圆柱 体 障 碍 物 ,然后 将 夹 持 器 移动 到 目标 木 块 上 方 , 最 后 将 
木 块 从 桌 上 推倒 在 地 。 整个 示 教 动作 连贯 完成 , 示 教 过 程 重复 10 次 。 在 示 教 过 程 中 ,所 关注 
的 3 个 关节 角度 (如 图 12. 6(a) 所 示 ) 在 每 个 时 刻 的 状态 都 会 被 记录 下 来 ,由 此 得 到 的 数据 用 于 训 
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(a) 示 教 者 (b) 仿真 场景 
图 12.6 示 教 者 及 V-REP 仿真 场景 


练 DMP 模型 。 为 了 与 DMP 模型 的 系统 自治 性 相 匹配 ,使 用 时 间 步 长 作为 运动 时 长 的 单位 ,并 将 
每 个 示 教 轨迹 的 时 长 步 数 转换 为 100 步 , 即 每 个 示 教 轨 迹 在 每 个 关节 上 会 有 100 个 数据 。 


图 12.7 示 教 过 程 


学 习 结 果 如 图 12. 8 所 示 , 可 以 看 出 ,对 于 每 个 关节 位 置 ,重新 生成 的 运动 轨迹 仍 保持 着 与 
示 教 轨迹 相似 的 形态 。 在 不 改变 生成 运动 的 起 始 位 置 和 终点 位 置 的 情况 下 ,在 仿真 Baxter 机 
器 人 上 测试 生成 的 运动 轨迹 ,结果 如 图 12.9 所 示 ,可 以 看 出 ,机 器 人 成 功 地 将 左边 的 木板 从 桌 
子 上 推翻 ,同时 也 没有 碰撞 到 柱子 和 球 。 

原始 的 DMP 只 用 于 学 习 单 个 示 教 轨迹 。 为 了 将 其 与 改进 的 方法 作 比 较 , 使 用 10 组 示 教 
数据 训练 得 到 10 个 DMP ,并 在 没有 修改 运动 终点 位 置 的 情况 下 将 其 应 用 到 机 器 人 上 ,其 中 6 
个 会 使 得 机 器 人 无 法 将 左边 木板 推倒 在 地 ,或 者 同时 推倒 了 两 个 木板 。 通 过 修改 10 个 DMP 
模型 中 的 目标 位 置 ,机 器 人 才能 全 部 成 功 完成 任务 。 

3) 基于 DMP 的 运动 技能 拓展 仿真 


示 。 从 图 中 可 以 看 出 ,修改 后 的 运动 轨迹 最 后 仍然 能 稳定 在 目标 位 置 。 为 了 检验 DMP 的 空 
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一 一 示 教 轨迹 ee 重复 轨迹 


1 DO 020405060 qe 0 oI00 
时 间 /s 
(a) 关节 1 

1.5 -=- 示 教 轨迹 we 重复 轨迹 


时 间 /s 
(b) 关节 2 


一 一 示 教 轨迹 ee 重复 轨迹 


0 0 320" 30 405550 60 70 00 90- “100 
时 间 /s 
(c) 关节 3 


图 12.8 学 习 结果 : 示 教 轨迹 及 新 生成 的 运动 轨迹 


图 12.9 机 器 人 复 现 所 学 运动 技能 
关节 1 
-一 -前 一 阶段 一 新 阶段 


0 10 20 30 40 50 60 70 80 90 100 
时 间 /s 


图 12.10 改变 关节 1 起 始 位 置 后 的 位 置 变化 曲线 
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间 拓 展 性 能 ,修改 运动 轨迹 的 目标 位 置 ,使 机 器 人 最 终 能 够 将 另 一 块 木板 从 桌面 上 推翻 落地 。 
3 个 关节 对 应 的 原始 目标 位 置 为 [9 ,0, ,9,]= 二 [一 0.755,0.652,0.664](rad) ,这 里 将 其 修改 为 
[一 0. 987,0.564,0.635](rad) ,以 使 得 所 学 运动 技能 的 目标 位 置 落 在 右边 的 木板 上 。3 个 关 
节 位 置 的 变化 过 程 如 图 12. 11 所 示 ,可 以 看 出 ,每 条 轨迹 都 保持 着 相似 形状 ,并 最 终 近 似 地 到 
达 目 标 位 置 。 另 外 ,我 们 在 仿真 机 器 人 上 测试 了 拓展 后 的 运动 技能 ,结果 如 图 12. 12 所 示 。 由 
结果 可 知 ,仿真 机 器 人 的 左手 臂 绕 过 了 障碍 物 并 成 功 地 将 右边 的 木板 推倒 。DMP 的 另 一 个 能 
力 是 时 间 缩 放 。 为 了 测试 其 性 能 ,将 时 间 缩 放 因 子 z 从 1 调整 为 0.5, 这 可 以 加 快 生 成 运动 的 
速度 。 如 图 12. 13 所 示 ,3 个 关节 在 时 间 步 数 为 50 时 到 达 目 标 位 置 ,仿真 机 器 人 以 接近 两 倍 
的 速度 完成 了 此 项 任务 ( 见 图 12. 14) 。 


----8goal=-0.76Srad 


goal=-0.987rad 


Oi/rad 


0 ‘10 ‘2%0 WW 0 00 TD 0 WW 10 
时 间 /s 
(a) 节 关 1 


1.0 -——- goal=0.652rad 


goal=0.564rad 


Os/rad 


1.0 1 | 1 
0 10 20 30 40 50 6 70 80 9 100 
时 间 /s 
(b) 节 关 2 
1.0 -——-goal=0.664rad goal=0.63Srad 


Oyrad 


lg “0 30 A400 S50 “00 “0-30 900 400 
时 间 /s 
(c) 节 关 3 


图 12.11 改变 目标 位 置 后 3 个 关节 的 位 置 变化 曲线 


图 12. 12 仿真 机 器 人 对 所 学 运动 技能 进行 空间 上 的 拓展 
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-一 -到 | 一 一 天 0.3 


Oi/rad 


1 i sie oe 一 一 上 一下 一 pp 一 
10 8 3 0 0 的 ”人 加 00 
时 间 /s 
(a) 节 关 1 


0/rad 


de i 1 上 1 :3 
0 10 20 30 4 和 人 和 人 0 .70 30 %0 100 
时 间 /s 
(b) 节 关 2 


-- -天 一 一 天 0.3 
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时 间 /s 
(c) 节 关 3 


图 12.13 改变 时 间 缩 放 因 子 后 ,3 个 关节 的 位 置 变化 曲线 


图 12.14 仿真 机 器 人 对 所 学 运动 技能 进行 时 间 上 的 拓展 


2. 基于 神经 网 络 的 前 馈 补偿 验证 实验 

未 知 环境 与 负载 使 我 们 无 法 事先 准确 获得 机 械 臂 的 动力 学 模型 ,进而 会 影响 机 械 臂 的 控 
制 效果 。 在 实验 中 ,使 用 神经 网 络 控制 算法 对 未 知 负载 产生 的 影响 进行 补偿 。 为 验证 其 效果 ， 
在 真实 的 Baxter 机 器 人 上 进行 一 组 对 照 实 验 以 比较 有 无 补偿 时 的 控制 效果 。 

该 实验 在 现实 的 Baxter 机 器 人 上 进行 。Baxter 机 器 人 左手 的 夹 持 器 上 持 有 一 个 水 杯 作 
为 负载 ,其 左手 末端 位 置 将 绕 着 一 个 水 平面 上 的 圆圈 做 周期 运动 ,其 位 置 轨迹 给 定 为 [x ,y ,zj]= 
[0. 65 十 0. lsin(2xt/6),0.2 十 0. lcos(2xt/6),0.2, 姿 态 的 欧 拉 角 则 固定 为 [x/2,0, 一 x/2], 如 


图 12. 15 所 示 。 对 应 每 个 位 置 的 关节 角度 通过 机 
人 句 人 逆 运 动 学 求 得 。 考 虑 到 未 知 负载 加 在 机 械 
辟 末 端 控制 器 时 ,其 重力 对 机 械 臂 动力 学 模型 的 
影响 最 为 明显 ,并 考虑 计算 机 计算 效率 问题 ,在 
这 个 实验 中 只 使 用 神经 网 络 去 允 近 机 械 辟 动力 
学 模型 中 的 重力 项 G。 对 于 每 一 维 输 入 ,选择 3 
个 节点 ,神经 网 络 权 重 初始 值 设 为 0。 

对 比 实验 设计 : 首先 ,在 不 加 神经 网 络 补偿 
的 情况 下 ,使 用 PD 控制 器 对 加 有 负载 的 Baxter 
机 器 人 进行 控制 ,使 其 按 既 定 轨迹 做 周期 运动 ， 
并 记录 每 个 关节 的 实际 位 置 以 及 位 置 误差 ,实验 
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图 12.15 实验 场景 


结果 如 图 12. 16 和 图 12.17 所 示 。 图 12. 16 为 未 加 神经 网 络 补偿 情况 下 的 关节 轨迹 跟踪 情 


况 ,虚线 表示 参考 轨迹 , 实 线 表 示 实 际 的 关节 运动 


轨迹 。 图 12. 17 为 未 加 神经 网 络 补偿 下 的 轨 


迹 跟 踪 误 差 。 然 后 ,加 上 神经 网 络 前 馈 补 偿 , 重 复 以 上 实验 ,实验 结果 如 图 12. 18 一 图 12. 21 
所 示 。 其 中 ,图 12. 18 为 神经 网 络 补偿 下 的 关节 轨迹 跟踪 情况 ,虚线 表示 参考 轨迹 , 实 线 表 示 
实际 的 关节 运动 轨迹 。 图 12. 19 为 神经 网 络 补 偿 下 的 轨迹 跟踪 误差 。 与 没 加 补偿 的 情况 相 
比 , 使 用 神经 网 络 前 馈 补偿 时 的 跟踪 误差 明显 减 小 ,整体 误差 范围 减 小 到 [一 0. 3,0. 3](rad) 。 
图 12. 20 为 神经 网 络 前 馈 补偿 力矩 ,其 在 最 后 与 参考 轨迹 一 样 呈 周期 变化 。 图 12. 21 为 神经 
网 络 权重 矩阵 每 列 的 范 数 , 从 中 可 以 看 出 神经 网 络 权重 整体 的 变化 趋势 。 
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图 12.16 未 加 神经 网 络 补偿 情况 下 的 关节 轨迹 跟踪 情况 (虚线 代表 参考 轨迹 ) 
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图 12.17 未 加 神经 网 络 补偿 下 的 轨迹 跟踪 误差 
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图 12.18 神经 网 络 补偿 下 的 关节 轨迹 跟踪 情况 (虚线 代表 参考 轨迹 ) 
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图 12.19 神经 网 络 补偿 下 的 轨迹 跟踪 误差 
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图 12.20 神经 网 络 前 馈 补偿 力 拢 
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图 12.21 神经 网 络 权 重 和 矩阵 每 列 的 范 数 
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3. 应 用 于 Baxter 机 器 人 的 技能 传递 拓展 实验 . 

接 下 来 使 用 真实 的 Baxter 机 器 人 验证 上 述 方法 的 空间 拓展 能 力 。 在 该 实验 中 , 示 教 者 通 
过 抓 住 Baxter 机 器 人 的 手腕 ,引导 其 手臂 运动 。 实 验 场景 如 图 12. 22 所 示 ,在 这 个 场景 中 ,机 
器 人 所 需要 学 习 的 运动 技能 是 将 杯子 移动 到 不 同 的 位 置 。 机 器 人 从 示 教 者 那里 学 习 如 何 将 杯 
子 从 桌子 的 A 点 移动 到 B 点 ,其 夹 持 器 的 运动 轨迹 如 图 12. 22(a) 所 示 。 然 后 将 运动 拓展 为 : 
将 杯子 从 A 点 移动 到 C 点 ,如 图 12. 22(b) 所 示 。 与 DMP 的 验证 仿真 相似 ,首先 利用 示 教 数 
据 训 练 DMP 模型 ,然后 机 器 人 再 对 运动 技能 进行 空间 上 的 拓展 。 训 练 结果 如 图 12. 23 所 示 ， 
从 图 中 可 以 看 出 ,DMP 模型 再 现 的 轨迹 能 够 使 机 器 人 成 功 将 杯子 从 A 点 移动 到 B 点 。 拓 展 
的 结果 如 图 12. 24 所 示 ,机 器 人 成 功 地 将 杯子 从 A 点 移动 到 C 点 ,同时 保持 着 与 示 教 动作 相 

似 的 运动 轨迹 。 


Se oa 
(a) A 点 移动 到 B 点 (b) A 点 移动 到 C 点 
图 12. 22 实验 场景 


出 发 (b) 移动 中 (©) 到 达 B 点 


图 12.23 机 器 人 复 现 所 学 运动 技能 


(a) 从 A 点 
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(a) 从 A 点 出 发 (b) 移动 中 
图 12.24 机 器 人 拓展 所 学 运动 技能 


12.1.5 小 结 

本 节 介 绍 了 一 个 人 机 示 教 系统 ,包括 人 到 机 器 人 的 运动 技能 传递 和 拓展 模型 设计 ,以 及 用 
于 运动 轨迹 跟踪 的 控制 器 设计 。 示 教 运动 使 用 了 动力 学 运动 基 元 模型 进行 建 模 。 结 合 高 斯 混 
合 模型 ,对 动力 学 运动 基 元 模型 进行 了 拓展 ,使 其 能 够 应 用 于 多 个 示 教 轨迹 的 学 习 。 通 过 这 个 
模型 ,实现 了 对 运动 技能 的 空间 拓展 和 时 间 拓 展 。 此 外 ,一 个 基于 神经 网 络 函 数 逼 近 技 术 的 控 
制 器 被 用 于 机 械 臂 控制 中 的 前 馈 补 偿 , 以 克服 未 知 负载 对 控制 效果 的 影响 。 最 后 ,通过 实验 和 
仿真 验证 了 该 方法 的 有 效 性 和 可 行 性 。 


12.2 机 器 人 自 适应 操作 技能 学 习 


机 器 人 通常 需要 从 人 类 的 示 教 中 泛 化 技能 ,以 满足 新 的 任务 要 求 。 但 是 ,在 以 下 情况 中 技 
能 泛 化 很 难 实现 , 即 : 

(1) 复杂 任务 包含 多 个 步 又; 

(2) 在 任务 再 现 过 程 中 机 器 人 需要 满足 一 些 约束 条 件 ; 

(3) 任务 复 现时 与 示 教 时 的 情景 有 较 大 差别 。 

本 节 将 介绍 一 个 机 器 人 技能 泛 化 的 框架 ,其 基本 思想 在 于 ,首先 将 要 学 习 的 技能 自动 分 割 


成 一 系列 子 技能 ,然后 相应 地 编码 并 对 每 个 单独 的 子 技能 (对 应 分 割 得 到 的 轨迹 片段 ) 进 行 调 “ 


节 。 具 体 来 说 ,单独 调整 每 组 分 段 运动 轨迹 ,而 不 是 调整 整个 运动 轨迹 ,从 而 使 其 易于 实现 技 
能 泛 化 与 拓展 。 此 外 ,在 实现 人 -机 器 人 可 变 阻 抗 控制 技能 转移 的 框架 中 ,将 基于 表面 肌 电 信 
号 估计 的 人 体 肢 体 刚度 考虑 进 该 框架 中 ,以 同时 实现 运动 轨迹 和 刚度 轨迹 的 拓展 。 

12.2.1 人 机 技能 泛 化 框架 | 

1. 框架 概述 

人 机 技能 泛 化 框架 包括 四 个 基本 阶段 : 示 教 、 分割 .对 齐 以 及 泛 化 。 框 架 如 图 12. 25 
所 示 。 

(1) 示 教 : 通常 只 考虑 示 教 者 的 运动 轨迹 ,机 器 人 只 能 学 到 人 手臂 的 位 姿 状 态 ,并 在 框架 
中 提取 人 类 示范 者 手臂 的 EMG 信号 以 提取 刚度 特征 。 为 了 捕获 尽 可 能 多 的 特征 ,通常 会 针 
对 一 项 任务 执行 多 次 示 教 。 
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(2) 分 割 : 将 运动 轨迹 以 及 刚度 分 布 表示 的 技能 分 成 子 技能 序列 。 通 过 这 种 方式 建立 了 
一 个 特定 任务 的 特征 库 。 

(3) 对 齐 : 具有 不 同 持续 时 间 的 示 教 配置 文件 通常 需要 在 时 间 上 对 齐 。 此 外 ,还 需要 对 
齐 来 自 不 同 部 分 的 坐标 点 。 

(4) 泛 化 : 然后 这 些 已 经 分 段 和 对 齐 的 运动 轨迹 和 刚度 分 布 序列 会 被 DMP 编码 。 最 后 ， 
根据 任务 情况 的 要 求 确 定 每 组 运动 轨迹 以 及 刚度 分 布 是 否 泛 化 。 显 然 , 当 没有 必要 泛 化 所 学 
习 的 技能 时 ,机 器 人 可 以 简单 地 模仿 示 教 者 的 运动 信息 。 


示 


和 
、 
杉 
全 
a 六 
一 hs AL 
交 LA \ 本 . 
a 
20 40 2 二 100 20 40 -60 80 100 


的 参考 


泛 化 


一 般 化 


-10 -0.5 00 05 10 


图 12.25 框架 的 图 形 表 示 


2. 使 用 BP-AR-HMM 进行 技能 分 割 
隐 马 尔 可 夫 模 型 (Hidden Markov Model, HMM) 是 一 种 基于 观测 数据 进行 时 间 序 列 分 析 
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的 统计 方法 。 通 过 使 用 前 向 -后 向 或 者 Viterbi 算法 有 效 地 估计 HMM 的 参数 ,并 且 生 成 观测 
数据 的 状态 序列 。 然 而 ,HMM 通常 需要 先 验 知识 来 选择 隐 含 数量 的 状态 ,因此 容易 导致 过 度 
拟 合 或 欠 拟 合 的 问题 。 这 在 很 大 程度 上 限制 了 模型 的 使 用 ,特别 是 在 处 理 复 杂 任 务 的 运动 分 
割 的 时 候 。 另 外 ,传统 的 HMM 不 适合 解决 根据 多 个 观察 数据 分 割 复杂 行为 的 问题 ,因为 它 
在 模型 中 独立 地 考虑 观察 结果 ,而 忽略 了 不 同 序列 之 间 的 关联 。 

Beta 过 程 自 回归 HMM (BP-AR-HMM) 可 以 改善 HMM 模型 的 两 个 缺点 。BP-AR- 
HMM 主要 包含 两 部 分 , 即 Beta 过 程 和 AR-HMM 模型 ,如 图 12. 26(a) 所 示 。 简 要 介绍 该 算 
法 的 基本 思想 如 下 。 、 

首先 ，Beta 过 程 (BP) 的 B 定义 了 可 能 数量 状态 的 全 局 权重 , 它 可 以 编码 许多 不 同 的 行 
为 ,参见 式 (12. 21) 和 式 (12. 22)。 特 征 包含 概率 w 和 状态 特定 参数 0, 二 {A ,3,}) 通 过 以 下 
过 程 生成 : 

B|B,~ BP(1,B,) C12 21) 


B= Dw,0, C1 D2 


k=1 
然后 ,针对 每 个 时 间 序 列 i 执行 参数 化 的 伯 努 利 过 程 (BeP) 以 生成 X, ,每 个 X; 用 于 生成 
二 进 制 向 量 了 ;二 [fi,f;s，…j ,表示 在 第 ;个 时 间 序 列 中 共享 哪些 全 局 特征 。 例 如 ,如 果 向 量 
f; 中 的 第 & 个 元 素 是 1, 即 f 二 1, 则 第 i 个 时 间 序 列 包 括 第 j 个 特征 。 因 此 ,这 使 得 多 个 时 
间 序 列 之 间 能 够 共享 全 局 特征 ,特征 选择 机 制 的 图 形 表示 如 图 12. 26(b) 所 示 。 
X, | BBeP(B) 1 23:) 


(12.24) 


Beta 过 程 AR-HMM 模 型 
(a) 特征 选择 机 制 


(b) 图 形 表示 
图 12.26 BP-AR-HMM 模型 


给 定 每 个 时 间 序 列 的 特征 指示 向 量 f; ,通过 具有 超 参 数 y 和 wk 的 Dirichlet 分 布 构 造 过 渡 
分 布 x;”。 因 此 ,第 i 个 时 间 序 列 可 以 从 具有 特征 约束 的 过 渡 分 布 r)” 的 全 局 库 中 选择 特征 。 
元 ”| 天 人 人 LY ry Fy fi) (IT2. 253 
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基于 转移 分 布 ,为 每 个 时 间 步 : 生成 状态 z,”, 如 下 式 : 
Zt A (12. 26) 


后 ,观察 量 > 可 以 使 用 式 (12. 27) 获 得 。 式 (12. 27) 表 示 向 量 自 回归 (Vector Auto 
VAR) 模 型 。 它 表明 ,给 定 VAR 模型 滞后 阶 数 x ,观察 值 为 模型 的 先前 7 个 观测 
值 的 线性 变换 的 总 和 加 上 状态 特定 的 噪声 。 


-4, a C12:279 


BP-AR-HMM 模型 只 需 几 个 参数 即 可 提供 可 靠 的 推理 ， Beta 过 程 以 完全 贝 叶 斯 方式 表示 
复杂 任务 的 潜在 特征 的 总 数 。 

本 节 中 ,BP-AR-HMM 模型 不 仅 用 来 分 割 多 次 示 教 产生 的 运动 轨迹 ,还 用 于 分 割 与 运动 
相对 应 的 人 体 手臂 刚度 轨迹 。 为 此 ,将 运动 信息 x 和 刚度 信息 p 封装 到 观测 值 中 : y' = 
{zi , 力 ，} ,使 得 它们 可 以 平行 同时 分 割 。 

3. 使 用 广义 时 间 规 整 算法 的 示 教 曲线 对 齐 

动态 时 间 规 整 (Dynamic Time Warping,DTW) 算 法 已 被 广泛 用 于 时 间 序 列 数 据 的 对 齐 。 
考虑 两 个 时 间 序 列 UU=[ui,…,upj 和 V 二 [vi ，… ,vej 在 维度 分 别 为 P 和 Q 的 情况 下 ,DTW 
是 获得 U 和 V 之 间 的 最 佳 对 应 关系 的 方法 ,使 得 给 定 的 误差 最 小 化 : 

Jaw = UR, —VR, 二? (CLD De 
其 中 ,R, 和 RR, 是 辅助 矩阵 。 该 过 程 可 以 看 作 是 在 参考 序列 U 和 观察 到 的 子 序 列 V 之 间 找 
到 对 齐 的 过 程 。 

一 般 机 器 人 从 示 教 中 学 习 技 能 所 获得 的 曲线 通常 是 多 维 时 间 序 列 。 在 这 种 情况 下 ,DTW 
算法 可 能 会 受 时 间 和 空间 计算 复杂 度 二 次 方 增长 的 限制 ,从 而 无 法 使 用 。GTW 算法 可 以 解 
决 上 述 问题 ,实现 人 体 运动 的 多 模 态 数据 对 准 。 考 虑 m 条 示 教 轨迹 , {Ul ,…,U,,)}, 且 U,; = 
[ul xy ],GTW 的 最 小 化 代价 函数 为 : 


m m 


Tie =2 > 1vIUw, 一 ww 有 + (Dow) + $v,)) (12. 29) 
1 一 1 


i=1j= 


其 中 ,W,; 和 VV， 分 别 是 非 线性 时 间 变换 和 低 维 空间 嵌入 短 阵 ， 2p(。) 和 多 (。) 是 正则 化 函数 。 
优化 成 本 函数 是 关于 对 准 ( 即 友 , ) 和 投影 矩阵 ( 即 w, ) 的 非 凸 优化 问题 ,其 分 别 通过 Gauss- 
Newton 算法 和 多 集 典 型 相关 分 析 来 求解 。 

4. 使 用 DMP 的 技能 表示 和 泛 化 

(1) DMP 模型 : 通常 ,一 维系 统 的 点 对 点 运动 DMP 由 3 个 微分 方程 表示 : 


人 C1230) 
磺 二 v (C12:31) 
T=—als (12. 32) 


若 忽 略 时 间 变 量 , 则 z, 可 用 >z 表示。 其 中 ,K 和 DD 分 别 是 恒定 刚度 系数 和 阻尼 系数 ; z+。 和 
zo 分 别 是 轨迹 的 目标 和 初始 点 ,rz 为 比例 因子 ; x 和 w 分 别 表 示 轨 迹 的 位 置 和 速度 ,它们 的 关 
系 如 式 (12. 31) 所 示 ; s 是 变换 系统 的 相位 变量 , 它 由 微分 方程 式 (12. 32) 表 示 ,ai 二 0 是 预定 
义 的 常数 。 
非 线性 强制 函数 f(s ; w) 采 用 以 下 表示 : 

fls; w)=w!g (12, 33) 
其 中 ,g 是 时 间 参 数 化 的 核 向 量 ,w 是 策略 参数 向 量 , 它 影响 学 习 轨 迹 的 形状 。 核 向 量 的 元 素 
定义 为 : 
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(s) 
Ls], = ss (12. 34) 
>)p, (Cs) 
尹 一 1 
使 用 归 一 化 基 函 数 p, (*) ,通常 定义 为 径 向 基 范 数 (RBF) 内 核 : 
WaS (12..35) 


其 中 ,c, 和 hh, 分 别 是 这 些 核 函数 的 中 心 和 宽度 。 通常 ,在 整个 轨迹 期 间 ,c， 在 时 间 上 是 等 间 
隔 的 ,并 且 刀 ,基于 经 验 来 选择 。 
策略 参数 w 可 以 使 用 (如 局 部 加 权 回 归 LWR 等 ) 监 督学 习 算 法 来 学 习 。 简 单 地 说 ,需要 
将 如 下 误差 函数 最 小 化 : 
min >) (Fase — f (5))? (12. 36) 
其 中 
coy RG. SL 
ine 人 
可 以 根据 示 教 的 轨迹 zz, ,这 , ,X, (t= 二 1,…,T) 和 xz, 二 x(T) 计 算 。 
(2) 用 于 编码 刚度 的 扩展 DMP: 为 了 同时 编码 从 示 教 中 获得 的 运动 轨迹 和 刚度 轨迹 , 扩 
展 DMP 模型 如 下 : 


(12. 37) 


eg 
二 gl1(TX,Ww,s) C12. 38) 


p=g2(p,y,s) 

式 (12. 38) 表 示 一 种 规范 系统 。 用 于 运动 轨迹 x 和 用 于 刚度 轮廓 p 的 两 个 变换 系统 具有 相同 
的 形式 ,但 具有 不 同 的 参数 w 和 7y。 它 们 由 相同 的 相位 变量 ; 驱动 ,从 而 保证 运动 轨迹 与 刚度 
轨迹 的 相位 同步 。 刚 度 参数 y 的 估计 过 程 与 w 的 估计 过 程 非常 相似 。 

5. 人 体 手臂 末端 刚度 到 机 器 人 阻抗 控制 器 的 映射 

在 任务 执行 期 间 , 为 了 捕获 人 体 手臂 肌肉 的 变化 信息 ,需要 实时 检测 示 教 者 手臂 的 肌 电信 
号 。 然 后 基于 EMG 信和 号 估计 人 体 手 臂 刚 度 。 最 后 ,将 刚度 映射 到 机 器 人 关节 阻抗 控制 器 ,如 
图 12. 27 所 示 。 


人 类 手臂 末端 刚度 


图 12.27 人 体 手 臂 末 端 刚度 与 机 器 人 关节 阻抗 控制 器 的 映射 


MYO 手 环 用 于 收集 原始 EMG 信号 ,然后 用 移动 平均 法 和 低 通 滤波 器 处 理 EMG 信号 。 
手 环 有 一 个 惯性 测量 单元 (IMU) ,所 以 可 以 使 用 两 个 MYO 手 环 以 确定 人 体 手 臂 的 关节 角度 : 


第 12 章 ”机 器 人 示 教 | 加 347 


sq 


一 个 佩戴 在 靠近 关节 扁 部 的 上 臂 , 另 一 个 佩戴 在 靠近 关节 肘 部 的 前 臂 。 该 工作 中 的 臂 配置 将 
用 人 体 手臂 三 角 模 型 描述 ,基于 该 模型 计算 人 辟 雅 可 比 行列 式 。 然 后 ,结合 人 的 雅 可 比 行列 式 
和 处 理 过 的 EMG 信号 ,离线 估计 人 体 手臂 关节 到 末端 刚度 模型 的 参数 。 在 示 教 阶段 ,将 人 体 
手臂 末端 刚度 在 线 映 射 到 机 械 臂 阻抗 控制 器 ,该 过 程 中 只 有 一 个 手 环 采集 EMG 信号 。 通 过 
这 种 方式 ,人 体 手臂 可 变 刚 度 被 传递 到 机 器 人 ,调节 机 械 臂 阻抗 用 以 适应 特定 任务 中 的 柔性 
要 求 。 

12.2.2 实验 验证 

1. 实验 装置 

人 形 双 臂 Baxter 将 用 于 研究 本 节 的 技能 学 习 实 验 。 关 于 Baxter 机 峰 人 的 详细 介绍 可 以 
”参考 前 面 章 节 。 在 力矩 控制 模式 下 可 以 调整 关节 刚度 和 阻尼 ,简化 为 PD 阻抗 控制 器 。 实 验 
装置 如 图 12. 28 所 示 。 


目标 


示 教 的 轨迹 一 一 生成 的 轨迹 mm 学 习 的 轨迹 
图 12.28 取水 任务 的 实验 装置 


在 示 教 期 间 , 机 器 人 的 主 臂 通过 物理 耦合 接口 直接 连接 到 示 教 者 的 一 个 臂 上 。 示 教 者 示 
教 提 水 任务 , 则 机 器 人 从 动 臂 会 跟随 主 臂 运动 。 示 教 者 手臂 上 佩戴 的 MYO 手 环 用 于 收集 实 
时 的 肌 电 信号 。EMG 信号 使 用 蓝牙 传输 发 送 到 计算 机 ,然后 进行 处 理 以 估计 人 体 手臂 末端 
刚度 。 随 后 ,使 用 UDP 协议 将 估计 的 刚度 曲线 适当 地 映射 到 机 械 臂 阻抗 控制 器 中 ,以 生成 机 
器 人 运动 控制 命令 控制 机 械 臂 运动 。 

2. 参数 设置 

首先 对 获得 的 示 教 轨迹 进行 预 处 理 ,使 得 每 个 维度 的 第 一 个 差异 的 方差 为 1, 平 均值 为 0。 
示 教 轨迹 为 8 维 时 间 序 列 ,其 中 7 维 为 运动 轨迹 ,另外 一 维 为 刚度 轨迹 。 所 有 的 示 教 轨迹 也 可 
以 按照 20Hz 的 频率 进行 二 次 采样 ,并 对 其 进行 平滑 处 理 。 

在 AR-HMM 模型 中 选择 1 阶 的 自 回 归 模 型 。Metropolis-Hastings 和 Gibbs 采样 器 运行 
10 次 ,每 次 迭代 1200 下 ,以 分 割 示 教 曲线 ,产生 10 个 分 段 。 根 据 10 次 运行 的 特征 设置 的 最 
大 对 数 似 然 性 进行 分 割 ,并 选择 这 些 分 割 后 的 轨迹 作为 后 续 处 理 的 运动 原 语 。 

3. 结果 和 分 析 

在 示 教 阶段 , 示 教 者 示 教 了 6 次 任务 。 在 这 项 工作 中 ,起 点 和 目标 都 在 所 有 示 教 中 都 是 固 
定 的 ,因此 运动 轨迹 的 形状 差异 不 大 ,因为 这 里 只 对 刚度 的 泛 化 感 兴趣 。 为 此 ,设置 机 器 人 在 


348 者 上 机 器 人 控制 一 一 运动 学 、 控 制 器 设计 、 人 机 交互 与 应 用 实例 


两 个 条 件 下 被 示 教 执行 任务 : (1) 将 半 瓶 水 提升 四 次 ,(2) 将 三 分 之 一 瓶 水 提起 两 次 。 由 于 人 
类 指导 员 肌 肉 激活 的 不 确定 性 ,即使 在 相同 的 任务 条 件 下 ,刚度 曲线 在 形状 上 也 不 完全 相同 。 

分 割 的 结果 如 图 12. 29 所 示 。 顶 行 显示 将 示 教 轨迹 分 割 为 标记 有 不 同 灰 度 细 条 的 子 技 
能 , 底 行 显示 覆盖 在 每 个 8 自由 度 示 教 轮廓 的 子 图 上 的 相应 分 割 片段 。 第 1.3、5、6 子 图 对 应 
于 第 一 任务 条 件 ,而 第 2.4 子 图 对 应 于 第 二 任务 条 件 。 可 以 看 出 ,BP-AR-HMM 可 以 成 功 地 
将 提 水 过 程 分 成 四 个 序列 ,基本 上 对 应 于 任务 的 相应 步骤 : (1) 到 达 瓶 子 位 置 ; (2) 拿 起 瓶子 
的 把 手 ; (3) 靠 近 障碍 物 ; (4) 抬 起 瓶子 越过 障碍 物 并 将 其 放置 在 目标 位 置 上 。 从 示 教 结果 可 
以 看 出 ,BP-AR-HMM 能 够 有 效 识别 多 个 示 教 中 的 重复 子 技能 ,即使 它们 出 现在 不 同 的 位 置 
并 对 应 不 同 的 刚度 轨迹 。 
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I I I I 下 I 
图 12.29 七 自由 度 运动 轨迹 和 一 自由 度 刚 度 轨迹 的 分 割 结果 。 六 个 子 图 中 ,每 个 图 的 第 一 行 
表示 刚度 ,其 他 行 均 表 示 关 节 角 ; 工 和 开 分 别 对 应 于 第 一 和 第 二 任务 条 件 


示 教 轨迹 的 对 齐 结 果 如 图 12. 30 所 示 。GTW 可 以 同时 对 齐 每 个 维度 的 轨迹 ,从 而 具有 
较 高 对 准 效率 。 灰 线 和 黑 线 分 别 表示 对 齐 的 轨迹 和 生成 的 轨迹 。 将 对 齐 的 轨迹 进行 平均 , 然 
后 使 用 DMP 模型 进行 建 模 ; 还 可 以 利用 一 些 统计 方法 来 生成 最 终 轨迹 。 可 以 看 出 ,所 有 示 教 
轨迹 即 运动 轨迹 和 刚度 轨迹 在 时 间 和 空间 中 都 可 以 很 好 地 对 齐 。 生 成 的 轨迹 可 以 捕获 六 个 示 
教 中 每 个 维度 的 特征 。 

随后 ,把 任务 泛 化 到 以 下 任务 情况 。(1) 抬 起 装 满 水 的 瓶子 越过 障碍 物 并 将 其 放置 在 目标 
上 。(2) 当 不 希望 高 刚度 的 时 候 , 保 持 低 刚度 控制 , 即 在 前 两 个 步骤 期 间 要 求 机 器 人 处 于 低 增 
益 控制 下 。 一 旦 获得 刚度 分 割 的 分 段 ,就 可 以 使 用 DMP 模型 有 效 地 调整 每 个 序列 。 为 了 处 
理 新 的 任务 情况 ,可 以 调整 第 三 和 第 四 段 序 列 的 刚度 ,即将 第 三 阶段 的 最 后 位 置 即 第 四 段 序列 
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6 8 1 
时 间 /s 
(b) 关节 S1 的 角度 


时 间 /s 
(d) 关节 E1 的 角度 
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时 间 /s 
(fj 关节 W1 的 角度 


S10 1 Ai 
时 间 /s 
(h) 1-DF 刚 度 分 布 


图 12.30 七 自由 度 运 动 轨迹 (a) 一 (g) 和 一 自由 度 刚 度 分 布 (h) 的 对 齐 结果 
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的 初始 点 调 到 某 个 值 。 基 于 参考 值 ( 带 圆 点 的 深 黑 色 线 ) 的 三 个 泛 化 的 刚度 曲线 (灰色 线 ) 如 
图 12. 31(a) 所 示 。 图 12. 30 所 示 的 位 置 曲 线 及 其 一 阶 导数 曲线 (速度 ) 被 用 作 可 变 阻 抗 控制 
器 中 的 参考 曲线 。 、 

使 用 泛 化 刚度 曲线 成 功 地 复 现 了 提 水 任务 。 在 > 轴 上 采集 的 机 器 人 末端 的 力 曲线 如 
图 12. 31(b) 所 示 。 灰 线 表示 从 六 次 示 教 中 测量 的 力 , 黑 线 表示 从 任务 复 现 阶 段 采集 到 的 一 个 
典型 的 力 分 布 情况 。 为 了 更 好 地 可 视 化 ,将 机 械 臂 末 端 测量 得 到 的 力 曲线 都 在 时 间 坐 标 上 对 
齐 。 力 曲线 和 刚度 曲线 具有 非常 相似 的 形状 ,这 意味 着 已 经 成 功 地 实现 了 人 -机 器 人 的 刚度 传 
递 。 更 重要 的 是 ,在 泛 化 阶段 期 间 的 力 达到 预期 : 在 第 一 步 和 第 二 步 中 保持 较 低 的 力 ,然后 在 
第 三 阶段 增 大 力 以 提取 满 瓶 的 水 ,从 而 完成 任务 要 求 。 


1.2 pon 
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(b) 六 次 示 教 ( 灰 ) 和 泛 化 阶段 ( 黑 ) 的 方向 上 的 力 曲线 


图 12.31 刚度 曲线 及 力 曲 线 


1 2 NE 


本 节 介 绍 了 一 种 PbD 框架 ,用 于 机 器 人 学 习 和 泛 化 人 类 的 变 阻 抗 技能 , 它 集 成 了 几 种 模 
型 ,分别 用 于 编码 、 分 割 \. 对 齐 和 泛 化 位 置 轨 迹 以 及 基于 EMG 的 刚度 曲线 。 该 框架 具有 学 习 
技能 库 的 潜力 ,包括 无 限 数量 的 运动 原 语 和 刚度 原 语 ,并 且 它 还 能 够 有 效 地 调节 每 一 段 所 展示 
的 曲线 ,这 方便 于 实现 技能 在 空间 与 力 空间 上 的 调整 与 泛 化 。 此 外 ,考虑 到 基于 示 教 者 肌 电 信 
号 的 刚度 估计 ,该 框架 允许 机 器 人 学 习 人 类 的 可 变 阻 抗 技 能 ,并 泛 化 类 似 技能 以 适应 新 的 任务 
情况 。 实 验 表明 ,该 方法 能 够 在 Baxter 机 器 人 上 学 习 和 泛 化 多 步骤 任务 。 另 外 ,该 框架 并 不 
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依赖 于 特定 的 硬件 平台 ,因而 也 适用 于 其 他 机 器 人 。 
12.3 基于 动态 运动 原 语 的 机 器 人 技能 学 习 与 活化 


传统 的 技能 学 习 和 泛 化 方法 没有 很 好 地 考虑 人 体 阻 抗 特征 (特别 是 刚度 泛 化 ) 的 情况 ,这 
使 得 技能 在 现实 人 机 交互 场景 中 不 够 类 人 化 并 且 受 任务 限制 。 本 节 将 介绍 一 个 技能 学 习 框 
架 , 它 使 机 器 人 能 够 有 效 地 学 习 人 类 的 运动 和 刚度 控制 策略 ,并 基于 Baxter 机 器 人 进行 实验 
验证 。 整 个 系统 框架 如 图 12. 32 所 示 。 


两 个 平行 的 泛 化 的 移动 
\ ， 


[ 
LW 
移动 轨迹 与 
aa 学 习 到 的 权重 


基于 DMPs 的 
示 教 阶段 框架 任务 泛 化 


自 适应 参数 


图 12.32 用 于 技能 学 习 和 泛 化 的 TbD 系统 框架 


本 节 介 绍 的 基于 肌 电 信号 的 可 变 阻 抗 技能 传递 与 基于 DMP 的 运动 规划 相 结合 的 方法 ， 
继承 了 两 者 在 机 器 人 技能 获取 方面 的 优点 。 更 重要 的 是 ,该 框架 可 以 用 一 种 统一 的 方式 同时 
编码 轨迹 和 刚度 曲线 ,实现 轨迹 泛 化 和 刚度 规划 。 


12.3; 上 4 志方 法 

1. 基于 肌 电 信号 的 可 变 刚度 估计 

为 了 使 机 器 人 能 够 从 示 教 者 那里 学 习 适 当 的 刚度 调节 策略 ,首先 需要 估计 示 教 者 的 肢体 
末端 刚度 ,从 示 教 者 的 手臂 提取 的 肌 电 信号 用 于 监测 肌肉 激活 程度 ,从 而 进行 刚度 估计 。 随 
后 ,估计 的 人 体 末端 刚度 作为 关节 阻抗 控制 器 中 的 参数 。 

笛 卡 儿 刚 度 与 人 体 手臂 刚度 之 间 的 映射 关系 可 表示 如 下 : 

9Jh" (Cg)F。。 
9d) 

其 中 ,KER'” 和 开 )ER"” 分 别 表示 人 体 手臂 末端 刚度 和 关节 刚度 ; Ji ER” 为 人 体 
手臂 的 雅 可 比 矩 阵 的 伪 逆 ,表示 人 体 手 臂 的 姿态 ; g, ER” 表示 手臂 关节 角度 ; F..ER“ 是 施 
加 在 人 体 手臂 末端 的 外 力 。 

一 般 而 言 ,人 体 手臂 的 关节 刚度 K; 和 肌肉 同步 收缩 活动 、 人 体 手臂 姿势 .肌肉 伸展 反射 
等 相关 。 这 里 采用 一 个 简化 的 估计 模型 ,其 中 ,肌肉 同步 收缩 指标 a(p) 和 固有 刚度 常数 KK， 
的 乘积 用 来 表示 关节 刚度 KK; ,可 以 在 最 小 肌肉 同步 收缩 的 情况 下 识别 ,也 就 是 K; 二 a (p) 
Kj;。 系 数 a(p) 可 以 由 以 下 公式 得 出 : 


We Ned [Ee ao | Ca) (12.89) 


Mt ee Py 


证 
其 中 ,A4， 和 4, 是 需要 估计 的 正常 系数 ,p 是 用 于 表示 肌肉 激活 程度 的 指标 。 指 标 p 可 以 通过 
两 步 获 得 : 低 通 滤波 器 (截止 频率 为 100Hz 的 有 限 脉 冲 响 应 滤波 器 ) 和 移动 平均 技术 。 首 先 ， 


使 用 MYO 手 环 从 每 个 通道 中 提取 原始 肌 电 信和 号 的 包 络 曲线 。 这 里 使 用 的 肌 电 信号 共有 8 个 


a(p) 一 1 十 (12. 40) 
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通道 。MYO 手 环 佩戴 在 上 臂 以 采集 肌肉 的 原始 肌 电 信号 数据 ,如 图 12. 33 所 示 。 肌 电信 号 
包 络 困 数 示例 如 图 12. 34 所 示 。 


一 包 络 线 : N*m/rad 
肌肉 收缩 — EMG: 0.1mV 


5 10 15 
时 间 /s 
图 12.33 刚度 估计 中 MYO 的 佩戴 位 置 图 12.34 通过 低 通 滤波 器 和 移动 平均 法 对 肌 电 信号 


进行 包 络 处 理 的 示例 ; 灰 线 为 原始 肌 电信 号 ， 
黑 线 表示 肌 电 信号 处 理 后 得 到 的 包 络 曲线 


移动 平均 法 表达 式 为 : 
W=1 
f (A) = 元 对 EMGCA， AN) (12. 41) 
k=0 


其 中 ,f(A,) 表 示 包 络 函数 的 幅 值 , W 表示 窗口 大 小 ,设置 40 个 样本 (采样 率 为 100Hz)。 
EMG(A% ) 表 明 从 通道 检测 到 的 原始 肌 电 信号 的 幅 值 。& 和 :+ 分 别 为 采样 点 和 时 间 。p 可 以 
通过 累加 肌 电 信号 包 络 函 数 幅 值 的 绝对 值 计 算得 到 ,如 下 : 


N 
p= | frCAs) | (12. 42) 
1 一 1 


因此 ,有 两 个 参数 , 即 常 系数 1 \Mz, 以 及 需要 估计 的 关节 刚度 固有 常 矩 阵 久 ) 。 这 些 参数 
可 以 通过 最 小 化 以 下 表达 式 来 估计 : 
Js 

9g, 
其 中 ,人 体 手臂 的 第 卡 儿 刚 度 为 KER""。 | 

人 体 手臂 雅 可 比 J， 负责 考虑 末端 刚度 曲线 的 几何 变化 。 为 了 计算 J ,人体 手臂 运动 学 - 
模型 ( 即 D-H 模型 ) 用 于 表示 简化 的 人 体 手臂 运动 学 。D-H 模型 将 人 体 手臂 表示 为 一 个 7 自 
由 度 的 机 械 臂 模型 : 肩 部 有 3 个 自由 度 , 肘 部 有 2 个 自由 度 , 手 胸 有 2 个 自由 度 。 

2. 用 于 示 教 的 双 辟 控制 方法 

在 示 教 阶段 ,采用 基于 遥 操 作 的 运动 学 示 教 方法 用 于 实现 人 -机 器 人 的 技能 传递 。 这 里 设 
计 了 双 臂 控制 策略 以 实现 示 教 者 任务 ,两 臂 中 的 一 个 用 作 主 臂 , 另 一 个 用 作 从 臂 ,通过 机 械 耦 
合 接口 把 主 臂 连接 到 示 教 者 的 手 上 。 示 教 者 带动 主 臂 ,机 器 人 从 臂 同步 跟随 动作 去 执行 特定 
任务 。 与 传统 的 示 教 者 可 以 直接 操控 和 驱动 从 臂 的 示 教 方式 相 比 ,这 种 间接 的 示 教 方式 ( 遥 操 
作 ) 可 以 使 示 教 者 更 自然 地 调节 其 肌肉 活动 及 相应 刚度 。 

在 机 器 人 双 臂 之 间 施 加 一 个 虚拟 的 弹簧 阻尼 系统 ,由 于 两 臂 之 间 存 在 运动 跟踪 误差 ,产生 
虚拟 阻力 作为 触觉 反馈 并 提供 给 示 教 者 。 因 此 , 示 教 者 能 够 实时 “感觉 ”运动 误差 ,因而 可 以 调 


a(pOK,) Ja (qn)K Ti (qn) ] (12. 43) 
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节 其 肌肉 力量 来 补偿 误差 。 

3. 用 于 技能 表征 的 动态 运动 原 语 模 型 

本 节 将 介绍 从 多 个 示 教 轨迹 中 学 习 的 动态 运动 原 语 (Dynamic Motion Primitives, DMP) 
模型 用 于 位 置 轨迹 自 适 应 调节 。 此 外 ,该 DMP 框架 也 被 扩展 用 于 刚度 自 适 应 调节 ,如 图 12. 35 
所 示 。 


权重 轨迹 
0.5h | 一 一 2 
Wo ; 
1.0 : 0 
0 rd 6 10 0 20 40 60 80 100 
0.5 
权重 刚度 
; 1.2 
0 1.0 1.0 
Time 0.8 
m0 
0.4 
02 
01s 0 50 4 和 0 60 80100 


图 12.35 DMP 系统 的 扩展 框架 ; 两 组 变换 系统 分 别 用 于 表示 位 轩 轨 迹 和 刚度 曲线 ， 
这 两 个 系统 由 相同 的 时 间 缩 放 系数 r 驱动 ,以 便 满足 相位 同步 


1) 编码 位 置 的 DMP 模型 
一 般 而 言 ,DMP 模型 可 以 分 为 两 种 类 型 , 即 离散 型 DMP 和 周期 型 DMP。 为 了 实现 实验 
任务 ( 即 切割 任务 ) ,使 用 前 一 种 类 型 。 典 型 的 DMP 模式 定义 如 下 : 
Ot = 人 (= (12. 44) 
过 二 vw C12. 45) 
其 中 ,x 表示 机 器 人 末端 或 关节 角度 的 位 置 ,v 是 速度 ,k 和 4 分 别 代 表 系 统 的 弹簧 和 阻尼 系 
数 ,r>0 时 为 时 间 缩 放 系数 ,z。 和 8& 分 别 表 示 初 始 位 置 和 目标 位 置 。f(s) 是 一 个 非 线 性 连续 
有 界 函 数 ,* 是 一 阶 动态 系统 的 状态 , 它 具有 相同 的 时 间 常 数 r。 
古人 [CE (12. 46) 
其 中 ,a, 表示 预定 义 常数 。 状 态 s 可 以 看 作 一 个 阶段 变量 , 它 被 设置 为 从 1 到 0 单调 递减 。 
由 于 DMP 模型 不 依赖 于 时 间 而 是 取决 于 状态 * ,使 得 它 可 以 在 不 改变 运动 轨迹 的 情况 下 推广 
模型 ,以 适用 于 其 他 情况 。 
非 线性 函数 f(s) 定 义 如 下 : 


N 
f(s5) = D7,giCs)s (12. 47) 


~ i=] 


exp(— hs(s —cC: > 
$; (二 (12. 48) 


Dope 人 一 3 


其 中 ,$; Cs) 被 认为 是 中 心 c; 和 宽度 的 标准 化 径 向 基 消 数 。N 表示 需要 在 模型 中 使 用 的 高 
斯 函数 的 数量 。 参 数 c; .h; 和 NN 是 根据 任务 的 情况 确定 的 。 
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参数 估计 是 监督 学 习 过 程 。 给 定 一 次 示 教 ,获得 目标 力 f(s): 
Tw ds A mE) 


PR he Eh tt (12. 49) 
8 Xo 到 
然后 ,使 用 局 部 加 权 回 归 (Locally Weighted Regression, LWR) 计 算 DMP 模型 的 权重 
7 为 : 
RE (12. 50) 


在 DMP 模型 中 ,通过 在 微分 方程 中 加 入 耦合 项 ,使 得 该 模型 能 够 在 静态 或 存在 移动 障碍 
物 的 情况 下 ,增加 系统 对 外 部 扰动 的 鲁 棒 性 。 

2) 编码 刚度 的 DMP 

要 平行 地 表征 轨迹 和 刚度 曲线 ,拓展 原始 的 DMP 可 以 描述 为 ， 


=) (12. 51) 
= 二 gl(z,s,Y) (12.'52) 
GY, (IT& 53) 


其 中 ,h(s) 是 正则 系统 。 式 (12. 52) 和 式 (12. 53) 代 表 两 个 变换 系统 ,一 个 用 于 编码 位 置 轨迹 ， 
另 一 个 用 于 刚度 曲线 。 注 意 ,这 两 个 变换 系统 由 同一 个 正则 系统 驱动 ,从 而 可 以 保证 相位 同 
步 。 这 意味 着 在 需要 时 可 以 为 机 器 人 手臂 提供 适当 的 刚度 。 使 用 符号 p 来 表示 刚度 ,因为 可 
变 刚度 是 从 肌 电 信和 号 中 评估 的 , 它 可 以 看 作 是 刚度 指标 。 

整个 DMP 系统 的 扩展 框架 如 图 12. 35 所 示 。 刚 度 DMP 的 计算 过 程 类 似 于 运动 DMP 的 
过 程 ,只 需要 稍 作 修改 , 即 用 p 替换 输入 x 并 选择 合适 的 参数 ,如 弹 得 和 阻尼 系数 ,以 及 基 峰 
数 的 数量 。 

为 了 从 多 次 示 教 中 学 习 权 重 , 可 以 采用 直接 的 方式 来 确定 权重 。 首 先 ,根据 其 相应 的 表 
现 , 用 1 到 10 的 分 数 评估 每 个 示 教 轨迹 。 对 于 每 个 示 教 数据 ,系数 x; 与 得 分 x; 相关 ,定义 
如 下 : 


Kk; 一 一 一 C12,54) 


=1 


其 中 ,L 为 示 教 次 数 。 最 终 的 DMP 输出 Q=={z ,区 ,Pp) 由 式 (12.55) 计 算 .: 
员 
0Q = Dr, (12. 55) 
i 一 1 


12.3.2 实验 验证 

实验 中 使 用 七 自由 度 Baxter 机 器 人 进行 验 
证 。 通 过 比例 微分 阻抗 控制 律 ,可 以 在 力矩 控制 
模式 下 修改 关节 刚度 和 阻尼 。 使 用 MYO 手 环 监 
测 示 教 者 的 肌肉 活动 以 收集 原始 肌 电信 号 。 切 割 
和 抓 取 -放置 (Pick-Place) 这 两 个 任务 用 以 验证 上 
述 方 法 的 有 效 性 。 

1. 切割 任务 

实验 装置 如 图 12. 36 所 示 。 在 示 教 阶段 ,机 
器 人 的 其 中 一 个 臂 作 为 主 臂 ,通过 耦合 装置 将 其 
连接 到 示 教 者 的 手臂 末端 。 另 一 个 臂 作 为 从 臂 ， 图 12. 36“ 示 教 期 间 切 割 任务 的 实验 设置 
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并 将 小 刀 作 为 切割 工具 安装 在 从 臂 末端 。 示 教 驱动 主 臂 移动 ,从 臂 跟随 移动 并 且 学 习 示 教 者 
的 手 的 位 置 轨 迹 和 刚度 曲线 。 在 再 现 阶段 ,从 臂 再 现 和 泛 化 所 学 习 的 技能 。 

首先 ,在 示 教 阶段 , 示 教 者 通过 遥 操 作 示 教 10 次 切割 操作 。 在 每 次 示 教 期 间 ,记录 七 个 关 
节 的 轨迹 以 及 示 教 者 的 肌 电 信号 。 然 后 ,根据 示 教 数据 训练 DMP 模型 。 这 里 采用 定性 标准 
来 确定 每 次 示 教 的 权重 。 如 果 在 从 臂 与 物体 接触 时 系统 不 稳定 ,或 者 在 示 教 过 程 中 最 终 没有 
很 好 地 切割 物体 , 它 将 获得 较 低 的 分 数 。DMP 模型 的 弹簧 系数 & 和 阻尼 系数 4 分 别 设 定 为 
150 和 25。 选 择 基 函数 的 数量 ; 轨迹 DMP 和 刚度 DMP 的 数量 分 别 为 100 和 50。 

1) 使 用 DMP 模型 进行 运动 路 径 规划 

在 这 个 子 任务 中 ,主要 验证 扩展 DMP 模型 的 技能 空间 泛 化 能 力 ,实验 结果 如 图 12. 37 所 
示 。 目 标 是 生成 已 学 习 技 能 以 在 不 同 空间 位 置 切割 物体 。 这 里 主要 是 指 沿 y 轴 在 不 同位 置 
切 黄瓜 。 机 器 人 是 在 基于 肌 电 信和 号 的 可 变 阻 抗 模 式 下 进行 控制 。 注 意 : 在 位 置 控制 模式 下 ， 
只 需要 规划 路 径 ,而 在 阻抗 控制 模式 下 ,路 径 和 速度 都 需要 调整 (轨迹 和 速度 的 泛 化 是 通过 改 
变 DMP 模型 的 轮廓 来 实现 ) 。 

从 臂 在 y 轴 上 被 测 端点 位 置 如 图 12. 37(c) 所 示 , 它 表明 端点 可 以 在 空间 上 泛 化 到 不 同 的 
位 置 。 执 行 切割 任务 的 结果 如 图 12. 37(d) 所 示 ,该 子 任务 表明 ,在 阻抗 控制 模型 中 ,技能 空间 
泛 化 的 DMP 模型 可 以 实现 。 


时 间 /s 时 间 /s 
(a) 运动 位 置 的 泛 化 (b) 运动 位 置 的 泛 化 


时 间 /s 
(0) 在 y 轴 方向 测量 的 机 器 人 末端 位 置 (d) 在 黄瓜 上 的 切割 位 置 ， 两 组 数字 (1 一 6) 表 
- 示 该 子 任务 成 功 执行 两 次 (一 次 切割 6 下 ) 


图 12.37 使 用 DMP 模型 进行 运动 路 径 规 划 。 第 一 次 切割 是 示 教 ,其 他 几 次 均 为 泛 化 切割 ， 
实 线 表 示 从 示 教 学 习 获 得 的 参考 轨迹 ,虚线 表示 泛 化 学 习 结果 


2) 刚度 规划 
该 实验 主要 验证 是 否 能 够 泛 化 刚度 轨迹 以 适应 新 的 任务 情况 。 为 此 ,基于 扩展 的 DMP 
模型 将 参考 刚度 指标 (参考 终 值 为 0.65) 拓 展 到 两 个 不 同 的 目标 ,例如 0.8 和 0.9。 在 该 子 任 
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务 中 切割 不 同 硬度 的 水 果 , 该 物体 比 在 示 教 阶段 使 用 的 水 果 具 有 更 厚 的 表皮 和 更 大 的 硬度 。 

对 于 上 述 3 个 条 件 ,每 一 个 条 件 机 器 人 都 要 执行 5 次 子 任务 ,成 功率 分 别 为 0/5、1/5 和 
5/5。 图 12. 38(b) 一 (f) 为 该 子 任务 的 其 中 一 个 结果 。 该 结果 表明 ,使 用 具有 参考 可 变 刚度 的 
阻抗 控制 模型 无 法 成 功 切割 物体 。 当 刚度 值 泛 化 到 0. 8 时 , 子 任务 仍然 无 法 完成 。 直 到 它 被 
调整 到 0. 9 ,才能 完成 切割 任务 。 从 以 上 结果 可 以 看 出 ,如 果 没 有 泛 化 到 合适 的 刚度 曲线 , 即 
使 在 变 阻 抗 控制 模式 下 ,机 器 人 也 缺乏 适应 性 ,这 与 人 在 完成 任务 时 的 经 验 比较 一 致 。 


时 间 /s 
(b) 


无 刚度 泛 化 第 1 次 刚度 泛 化 第 2 次 刚度 泛 化 
(d) (©) (OD 
图 12.38 a) 为 对 不 同 目标 的 刚度 指标 曲线 的 泛 化 ,(b) 和 (c) 分 别 表示 在 x 轴 方 向 上 测量 的 
辟 末 端 执 行 器 的 位 置 和 力 曲 线 ,(d)、(e) 和 (f) 分 别 表示 使 用 参考 刚度 曲线 、 
第 一 个 泛 化 刚度 曲线 和 第 二 个 泛 化 刚度 曲线 切割 时 的 不 同 切口 


2. 抓 取 - 放 置 任务 

抓 取 -放置 任务 的 实验 装置 如 图 12. 39 所 示 , 实 验 使 用 两 个 大 小 相同 (1l0cm X 10cm X 
10cm) .重量 不 同 的 对 象 1 和 对 象 2( 对 象 1 和 对 象 2 的 重量 分 别 为 0. 83kg 和 1. 40kg) 。 该 任 
务 的 实验 过 程 与 切割 任务 类 似 。 在 示 教 过 程 中 ,同一 个 示 教 者 教 机 器 人 提取 物体 1, 越 过 障碍 
物 ( 高 18cm) ,最 后 将 其 放 在 目标 上 (红色 十 字 标 记 )。 需 要 示 教 者 以 尽 可 能 低 的 高 度 抬 起 物 
体 ,以 使 其 越过 障碍 物 且 节 省 能 量 。 在 给 定数 量 的 示 教 之 后 ,可 以 生成 位 置 轨迹 和 刚度 曲线 。 
DMP 模型 的 参数 设置 与 切割 任务 中 的 相同 。 

在 再 现 阶段 ,机 器 人 需要 在 3 种 不 同情 况 下 执行 任务 。 子 任务 1: 使 用 示 教 的 刚度 曲线 
( 即 路 径 空间 规划 ) 将 物体 1 抬 起 并 放 到 新 的 目标 位 置 (黑色 十 字 标 记 ) ,距离 旧 目 标 5cm; 子 
任务 2: 使 用 示 教 的 刚度 曲线 ( 即 没有 刚度 规划 ) 将 物体 2 抬 起 并 放 到 新 的 目标 位 置 ; 子 任务 
3: 使 用 泛 化 的 刚度 曲线 将 物体 2 提起 并 放 到 新 目标 位 置 。 

每 个 子 任务 执行 7 次 。 在 第 一 个 子 任务 中 获得 了 7/7- 的 成 功率 ,这 表明 技能 空间 泛 化 已 
经 成 功 实现 。 机 器 人 分 别 以 1/7 和 677 的 成 功率 完成 了 第 二 个 任务 和 第 三 个 子 任务 。 两 个 不 
同 的 子 任务 表明 ,本 节 设 计 的 刚度 规划 框架 是 有 效 的 。 图 12. 40 为 一 次 示 教 和 三 个 子 任务 的 
执行 结果 。 

3. 分 析 

只 通过 调整 DMP 模型 的 参数 就 成 功 地 执行 了 切割 任务 中 的 子 任 务 2 和 抓 取 -放置 任务 中 
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-===--- Demo 一 一 一 一 泛 化 的 轨迹 生成 的 轨迹 
图 12.39 抓 取 -放置 任务 的 实验 装置 


I -(a) I -(b) LE E I -(d) I -(e) 


I -(a) I -(b) IT-(o) I-(d) I-(e) 


NV-(a) NV-(b) IV-(c) NV-(d) IV-(e) 


图 12.40 抓 取 -放置 任务 的 实验 结果 。 工 -(a) 一 (e)、T-(a)~(e)、 开 -(a) 一 (e 和 KM-(a) 一 (e) 分 别 
表示 示 教 、 子 任务 1、 子 任务 2 和 子 任务 3 的 典型 示例 


的 子 任务 3, 而 不 需要 再 次 教 机 器 人 。 男 外 ,可 以 基于 所 提出 的 框架 轨迹 和 刚度 曲线 的 不 同 组 
合 满足 任务 情况 的 不 同 要 求 。 更 具体 地 ,通过 把 相同 刚度 曲线 与 不 同 运 动 轨迹 组 合 在 一 起 ,可 
以 实现 在 可 变 阻 抗 控 制 模式 下 的 技能 空间 泛 化 (切割 任务 中 的 子 任务 1 和 抓 取 -放置 任务 中 的 
子 任务 1) ,并 且 通 过 把 相同 运动 轨迹 与 不 同 刚度 曲线 组 合 在 一 起 ,可 以 实现 刚度 规划 (切割 任 
务 中 的 子 任务 2) ,或 者 可 以 同时 实现 轨迹 和 刚度 曲线 ( 抓 取 -放置 任务 中 的 子 任务 3) 。 基 于 肌 
电信 号 的 刚度 估计 ,可 获得 用 于 刚度 泛 化 的 参考 刚度 曲线 ,人 体 末端 刚度 估计 方法 则 可 用 于 阻 
抗 调节 。 因 此 ,一 方面 可 以 方便 ` 有 效 地 实现 刚度 传递 和 泛 化 ; 另 一 方面 ,参考 刚度 曲线 在 很 
大 程度 上 取决 于 提取 的 肌 电 信号 的 精度 和 刚度 估计 模型 。 我 们 可 以 采用 更 完整 的 刚度 估计 模 
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型 来 提高 估计 的 参考 刚度 曲线 的 精度 。 

关于 使 用 多 个 示 教 数据 来 生成 参考 轨迹 和 刚度 曲线 的 方式 ,如 图 12. 41 所 示 。 为 简单 起 见 ， 
采用 定性 的 方法 确定 每 次 示 教 的 权重 。 此 外 ,不 同 的 示 教 者 在 技能 传递 过 程 中 示 教 的 技能 也 不 
尽 相 同 。 因 此 ,考虑 不 同人 之 间 的 差异 , 即 学 习 不 同 的 示 教 者 ,可 以 帮助 机 器 人 更 好 的 完成 任务 。 


0.05; Demo 的 目标 ， wy 
0 "7 


1000 1500 2000 2500 
时 间 序 列 
(a) 参考 轨迹 、 (b) 刚度 曲线 


图 12.41 抓 取 -放置 任务 的 参考 轨迹 和 刚度 曲线 


12.an35 ww 


本 节 介 绍 了 一 个 用 于 学 习 和 泛 化 类 人 化 变 阻 抗 操作 技能 的 示 教 框架 ,该 技能 学 习 框架 集 
成 了 基于 肌 电 信号 的 变 阻 抗 控制 和 DMP 模型 的 优点 。 首 先 , 通 过 提取 示 教 者 的 上 肢 肌 电信 
号 以 捕获 其 肌肉 刚度 特征 ; 然后 ,估计 人 体 手 臂 末 端的 刚度 曲线 用 作 阻 抗 控制 器 中 的 可 变 增 
益 。 通 过 这 种 方式 , 示 教 者 能 够 自然 且 直 观 地 把 特定 技能 的 阻抗 特征 传递 给 机 器 人 。 


12.4 基于 EMG 估计 的 肌肉 疲劳 度 在 机 器 人 示 教 中 的 应 用 


本 节 将 介绍 一 种 基于 疲劳 感知 的 机 器 人 技能 学 习 方法 ,该 方法 在 人 机 交互 过 程 中 考虑 了 
人 体 肌肉 疲劳 对 技能 学 习 的 影响 。 


12.4.1 基于 EMG 评估 肌肉 疲劳 


在 多 次 示 教 的 过 程 中 ,人 类 示 教 者 会 感到 疲劳 , 它 主要 由 肌肉 的 疲劳 程度 表现 出 来 ,并 将 
影响 机 器 人 学 习 的 质量 。 肌 电信 和 号 的 变化 可 以 描述 潜在 的 肌肉 活动 。 研 究 表明 ,肌肉 长 度 的 
变化 会 导致 肌 电 信和 号 的 振幅 和 频率 在 疲劳 运动 过 程 中 发 生变 化 。 本 节 介 绍 EMG 中 的 内 在 振 
幅 和 频率 与 人 体 肌肉 疲劳 的 关系 。 

对 于 带宽 非常 小 的 带 限 信号 ,例如 EMG 子 带 信号 , 它 占据 信号 功率 中 大 部 分 的 频率 分 
量 , 而 信号 的 主要 特征 为 该 分 量 的 幅 值 和 频率 ,可 定义 如 下 : 

Cn) =A(n)cosLO(n)j+ yn) 


=AGeos| Qn + Pan) 十 7Cz) (12.56) 


r 一 1 


其 中 ,A (n) 和 8B(n) 分 别 表 示 信 号 z(z) 的 振幅 和 相位 序列 ,2(z) 王 9(0z) 一 9(z) 一 0. 十 q(2) 一 


( 严 )/。 +eco) 为 明 时 频率 表示 采样 频率 (200Hz) ,gq (n) 则 表示 调频 变量 ,n(n) 表 示 可 能 


的 噪声 和 误差 。 
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me 


如 图 12. 42 所 示 为 从 带 通 滤波 中 获得 的 一 段 EMG 信号 及 其 子 带 。 在 每 个 子 带 中 ,估计 
出 的 包 络 序列 用 虚线 表示 。 在 短 时 间 间 隔 内 ,计算 包 络 序列 的 平均 值 ,以 表示 子 带 的 主要 幅 
值 。 由 于 采样 频率 的 限制 ,这 里 选择 3 个 频段 ( 即 小 于 10Hz .10 一 50Hz 以 及 大 于 50Hz) 对 信 
号 分 解 。 分 别 命 名 3 个 频段 子 带 信 号 的 平均 瞬时 振幅 为 AIEB' 0 、AIEN 5 和 AIEt0"00], 
图 12. 42 中 的 参数 分 别 为 AIEW*!J=39. 55,AIEW50] 一 22. 29 和 AIEL5%*10] 二 16. 81。 图 12. 43 
中 ,采集 了 EMG 子 带 信号 的 瞬时 频率 ,并 将 主 频 率 分 量 标识 为 瞬时 频率 序列 随时 间 的 平均 
值 。 同 样 地 ,分 别 将 它们 命名 为 AIF"" 、AIFR0 和 AIFB 0200 。 检 测 到 的 主 频 率 分 别 为 
4.57Hz、31. 36Hz 和 62. 95Hz。 


sEMG 信 号 
基 300 
车 200 
了 颈 100 I 

0 100 200 300 400 
样本 (n) 

子 带 1: [0, 10Hz] 
ee 
.39.55 
旦 
器 oo 

< 

子 带 2: [10Hz 50Hz] 
> 
总 
中 
联 

-A000 100 200 300 400 


样本 (nm) 


子 带 3 : [50Hz, 100Hz] 


> 
ND 
如 16.81 
附 
-1501 5 0 
样本 (n) 

一 一 SEMG 子 带 信 号 

--- 包 络 线 

~-- 包 络 线 的 均值 


图 12.42 EMG 子 带 信号 及 其 包 络 线 (虚线 ) ,每 个 子 带 中 点 虚线 表示 其 主 振幅 


低频 率 段 的 EMG 信和 号 在 识别 肌肉 疲劳 方面 比较 高 频率 段 的 信号 更 有 效 。 下 面 将 讨论 低 
频段 的 EMG 对 疲劳 效应 的 影响 。 

动态 运动 原 语 (DMP) 是 非 线性 动态 系统 构成 的 运动 单元 或 短 节 段 。 机 器 人 关节 空间 可 
以 由 多 个 DMP 表示 ,每 个 DMP 表示 一 个 关节 状态 ,这 里 使 用 离散 DMP 来 表示 关节 空间 上 的 
轨迹 。 关 于 DMP 模型 的 详细 介绍 请 参考 12. 1.2 节 。 


12.4.2 ”机 器 人 从 多 次 示 教 中 学 习 
这 里 采用 高 斯 混合 模型 对 多 次 示 教 的 数据 进行 建 模 。 
每 个 单独 的 示 教 试验 产生 一 组 机 器 人 的 关节 角 。 因 此 ,数据 集中 有 N 组 关节 角度 {0,.;， 


0 ,0 ,} J _， ,其 中 入 为 示范 性 试验 次 数 。 在 第 i 次 试验 中 ,0, ,ER 为 关节 角 , 工 , 为 示 教 
时 间 。 此 外 ,s, ,ER 为 系统 在 时 间 步 长 为 : 时 的 状态 ,同时 根据 对 应 的 EMG 数据 计算 出 肌肉 
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子 带 1: [0, 10Hz] 


子 带 3: [50, 100Hz] 


100 200 300 400 
样本 (n) 
一 瞬时 频率 
-~- 瞬时 频率 的 均值 


图 12. 43、 子 带 信号 的 瞬时 频率 和 主 频率 


疲劳 因子 MF,; ,其 中 i=1,…,N。 
为 了 从 数据 向 量 集 {s ,f) 中 学 习 , 其 中 了 为 系统 中 函数 f(s) 的 值 ,采用 高 斯 混合 模型 表示 
联合 分 布 P(s,f) 如 式 (12.5) 所 示 。 式 中 的 未 知 参 数 可 以 通过 期 望 最 大 化 算法 计算 得 到 。 相 
比 于 传统 的 高 斯 分 量 ,这 里 建议 采用 加 权 高 斯 分 量 分 析 肌 肉 疲劳 的 影响 。 考 虑 到 示 教 阶段 的 
疲劳 状态 都 不 相同 ,所 有 示 教 数据 的 权重 必须 相应 地 变化 。 因 此 ,与 传统 的 GMMs 算法 相 比 ,加 
权 GMMs(W-GMMs) 的 训练 数据 首先 按 疲 劳 因 子 进行 缩放 。W-GMMs 的 修正 EM 算法 如 下 : 
(1) 初始 化 : 设置 参数 的 初 值 。 
(2) 第 巨 步 ; 计算 第 个 高 斯 模型 的 响应 ,k= 二 1,2,…,K ,表达 式 如 下 : 
_ MF jag (zx; peg si 


K 
2 MF arg x; | Au » 24) 
k=1 


其 中 ,zj; 二 {57 ,fj ) ,MF; ,j= 二 1,2,…,NN 分 别 为 第 j 次 示 教 的 数据 向 量 和 肌肉 疲劳 因子 。 
(3) 第 M 步 : 在 新 一 轮 的 迭代 中 ,通过 极 大 似 然 估计 更 新 模型 参数 为 : 
N 4 


(12..987) 


Yn 


By 
A 了 到 水 
Hr 一 N 
3 
j= 
N 
全 2 
之 C27 — HEY 
后 (12. 58) 
之 / OO 
2 
y= 
N 
2 
Q, = 
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(4) 重复 第 已 步 和 第 M 步 直到 收敛 。 
通过 高 斯 混合 回归 (Gaussian mixture regression,GMR) 算 法 可 以 估计 函数。 在 GMR 
中 ,条 件 概 率 PCFls) 由 以 下 公式 给 出 : 


K 
P(f |s) ~ DBN GD, 62) (12. 59) 
k=1 


其 中 : 
nD 二 人 六 也 (2 Gs ph 


六 = 7 Zp (2g ) 
ON (GS Ms? 2, 
A 
| DN pr 
2 
根据 高 斯 分 布 的 线性 变换 属性 ,PC(f1s) 可 以 近似 为 P(f1s) 一 N ($6 ), 同 时 他 = 
K 
uPihse = ae, 其 中 信 是 期 望 。 
k=1 = 
最 后 ,对 非 线性 函数 人 


(12. 60) 


f(s) =$= 3 (ior SC (12. 61) 
k=1 


其 中 , As) 的 界限 为 PE[0,1] 和 sE(0,1]。 


12.4.3 实验 与 结果 
本 节 通 过 一 个 由 示 教 阶段 ,学习 阶段 和 再 现 阶 段 组 成 的 机 器 人 示 教 实验 对 上 述 方法 进行 
We, 


. 平台 设置 
果 Baxter 机 器 人 对 该 方法 进行 验证 。 在 实验 中 ,人 类 示 教 者 拖 动 机 器 人 末端 执行 器 完 
成 示 教 任务 ,在 此 过 程 中 记录 机 器 人 关节 角度 。 a 


这 里 使 用 MYO 手 环 采集 EMG 数据 ,如 图 12. 44 
所 示 。 在 操作 过 程 中 ,将 MYO 手 环 戴 在 示 教 者 的 手 
臂 上 以 记录 示 教 过 程 中 产生 的 肌 电 信号 ,其 默认 采样 
频率 为 200Hz。MYO 手 环 由 8 个 EMG 电极 和 9 轴 
惯性 测量 单元 组 成 ,这 些 电 极 可 以 收集 佩戴 者 手臂 肌 
肉 的 生物 电 变 化 。 将 采集 到 的 EMG 通过 蓝牙 传输 到 
客户 端 计算 机 。 

2. 示 教 阶段 

在 Baxter 机 器 人 的 末端 执行 器 上 固定 一 支 笔 , 笔 
的 起 始 位 置 为 障碍 物 ( 扒 县 的 圆 简 ) 的 顶部 ,如 图 12. 45 
所 示 。 在 人 类 示 教 者 的 拖 动 下 ,机 器 人 将 完成 以 下 动作 。 首 先 , 未 端 执 行 器 上 的 笔 戳 中 第 一 个 
圆 (A); 然后 它 绕 过 障碍 物 , 惟 中 第 二 个 圆 (B); 然后 它 继续 绕 过 障碍 物 , 惟 到 第 三 个 圆 (C)， 
最 后 回 到 它 的 起 始 位 置 。 因 此 ,钢笔 经 过 的 路 径 是 A 一 0 一 B00 一 C 习 0。 

3. 学 习 阶 段 和 再 现 阶段 

因为 人 体 手臂 的 运动 主要 由 肽 二 头 肌 了 驱动 ,所 以 实验 中 用 于 疲劳 估计 的 EMG 数据 来 自 
附着 在 肪 二 头 肌 上 的 传感器 。 在 30 次 示 教 中 ,提取 由 MYO 手 环 第 二 通道 捕获 的 EMG 子 带 


图 12.44 佩戴 了 MYO 手 环 的 示 教 者 
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[0,10Hzj] 的 瞬时 振幅 序列 ,并 计算 该 时 变 序列 在 每 2s 时 间 间 隔 内 的 平均 值 ,以 获得 疲劳 参 
数 。 为 了 体现 最 显著 的 趋势 和 消除 波动 ,这 里 使 用 一 个 3 阶 多 项 式 函 数 来 拟 合 数据 样本 AIE 
[0,10]。 经 过 平滑 处 理 的 所 有 试验 ( 共 30 次 ) 数 据 样本 的 曲线 如 图 12. 46 所 示 , .将 图 中 立方 
函数 的 拐点 定义 为 疲劳 度 的 出 现 点 (OF)。 在 训练 W-GMMs 时 ,选取 6 组 数据 用 于 学 习 新 的 
机 器 人 运动 轨迹 ,其 中 3 个 来 自 OF 之 前 的 示 教 ,其 他 的 来 自 OF 之 后 的 实验 。 在 W-GMMs 
训练 过 程 中 ,以 肌肉 疲劳 因子 为 权重 ,分 别 计算 并 归 一 化 为 0.9、0.8、0.7、0.4.、0.3.0.2。 


13.5 
13.0 
站 12.5 
起 
如 12.0 
堪 
区 11.5 
民 
11.0 
0 5 10 15 20 25 30 
示 教 的 次 数 
图 12.45 示 教 实验 场景 图 12.46 示 教 实验 中 ,疲劳 相关 特征 ( 肌 电 信号 的 
平均 瞬时 振幅 ) 的 平滑 曲线 


每 次 示 教 时 记录 的 持续 时 间 和 数据 量 各 不 相同 。 在 图 12. 47(a) 中 ,随机 选取 3 个 数据 样 
本 进行 观察 , 即 肩 关 节 S0 的 角度 。 结 果 表 明 ,它们 存在 时 间 上 的 偏差 。 因 此 ,采用 动态 时 间 规 
整 算法 (DTW) 将 数据 在 时 间 轴 二 对 齐 。 利 用 DTW 处 理 后 的 对 齐 数据 如 图 12. 47(b) 所 示 。 


0 500 1000 1500 “3000 2500 3000 
样本 (n) 
(a) 
一 样本 1 --- 样 本 2 …… 样本 3 
0.4 
其 0.2 
王 
es 
六 -02 
= 500 1000 1500 2000 2500 3000 3500 4000 
样本 (n) 
(b) 


-一 样本 1 --- 样 本 2 …… 样本 3 
图 12.47 (a) 肩 关节 角 So 的 3 个 原始 样本 (b) 经 DTW 处 理 后 的 样本 


如 前 所 述 , 将 OF 之 前 的 3 组 数据 、OF 之 后 的 3 组 数据 以 及 它们 对 应 的 权重 共 6 组 数据 
作为 训练 数据 构建 W-GMM。W-GMM 的 学 习 过 程 如 图 12. 48 所 示 ,其 中 图 12. 48(a) 为 关节 
空间 的 训练 数据 样本 ; 图 12. 48(b) 为 训练 后 的 GMMs, 对 16 个 高 斯 分 量 的 联合 分 布 进行 编 
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码 ; 图 12.48(c) 为 W-GMM 的 学 习 结 果 , 其 中 用 虚线 表示 训练 样本 。 观 察 到 ,在 GMMs 训练 
中 加 入 与 疲劳 相关 的 权重 ,权重 越 大 的 训练 数据 在 训练 结果 中 越 重要 ,这 意味 着 出 现 较 少 疲劳 
时 的 示 教 对 机 器 人 学 习 结 果 的 影响 更 大 。 


0.5 
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2 
六 
06 
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图 12.48 W-GMMs 的 学 习 过 程 


在 机 器 人 学 习 实 验 中 ,考虑 了 所 有 (7 个 ) 关 节 的 角度 数据 ,包括 2 个 肩 关 节 SO 和 S1,2 个 
肘 关节 -E0 和 El1,3 个 腕 关节 W1、W2 和 W3。W-GMMs 中 每 个 关节 的 学 习 结 果 如 图 12. 49 
所 示 。 在 接收 到 这 些 关节 角度 数据 后 ,Baxter 机 器 人 成 功 地 完成 了 人 类 示 教 者 的 示 教 任务 。 
Baxter 机 器 人 末端 执行 器 的 轨迹 如 图 12. 50 所 示 。 
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图 12. 49 Baxter 机 器 人 右 臂 的 7 个 关节 角 12. 50 ”Baxter 机 器 人 末端 执行 器 的 轨迹 
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T1224.4 小 结 


对 于 机 器 人 来 说 ,从 人 类 示 教 中 学 习 操作 技巧 是 一 种 直接 而 有 效 的 知识 传递 方式 。 但 是 ， 
这 种 操作 会 导致 示 教 人 员 感 到 疲劳 ,尤其 是 对 于 复杂 的 高 负荷 任务 。 本 节 介 绍 了 在 人 机 交互 
操作 环境 下 肌肉 疲劳 效应 的 研究 。 首 先 ,通过 肌 电 手 环 MYO 获得 人 类 示 教 者 的 表面 肌 电 信 
号 (EMG) 数 据 , 然 后 通过 收集 到 的 EMG 信号 产生 与 疲劳 相关 的 参数 。 这 些 用 于 检测 肌肉 疲 
劳 的 参数 通常 位 于 EMG 信号 的 低频 段 。 我 们 估计 子 带 信号 的 瞬时 振幅 值 和 频率 值 ,这 些 变 
量 是 示 教 者 疲劳 状态 的 观察 指标 ,可 在 机 器 人 轨迹 学 习 过 程 中 作为 参考 。 一 个 复杂 的 机 器 人 
学 习 程 序 需要 多 次 人 类 示 教 ,而 检测 到 的 肌肉 疲劳 状态 构成 了 在 不 同 试验 中 进行 权衡 的 基础 。 
最 终 的 学 习 结 果 将 通过 模型 适应 技术 由 加 权 训 练 样本 产生 。 


12.5 本 章 小 结 


示 教 技术 是 机 器 人 获得 人 类 运动 轨迹 或 操作 技能 的 一 种 行 之 有 效 的 方法 。 本 章 主要 介绍 
了 几 个 不 同类 型 的 示 教 技术 ,如 运动 技能 的 捕捉 与 传递 .基于 人 体 表 面 肌 电 信和 号 的 疲劳 度 进行 
人 机 示 教 等 ,讨论 了 机 器 人 示 教 学 习 过 程 中 的 建 模 、 学 习 \ 泛 化 ,控制 等 问题 。 关 于 机 器 人 示 教 
技术 还 有 很 多 其 他 应 用 场景 和 示 教 方法 ,这 里 只 介绍 当前 较为 热门 的 几 个 研究 方向 , 感 兴趣 的 
读者 可 以 结合 自己 的 研究 课题 进行 深入 研究 。 


其 他 应 用 


CHAPTER 13 


机 器 人 控制 技术 除了 在 人 机 交互 机 器 人 示 教 和 遥 操 作 等 方面 的 应 用 ,还 有 一 些 其 他 应 
用 ,本 章 将 结合 具体 的 研究 课题 进一步 阐述 这 些 应 用 。 


13.1 基于 阻抗 匹配 策略 的 物理 人 机 交互 控制 


本 节 将 介绍 一 种 能 够 实现 人 与 机 器 人 末端 执行 器 之 间 直 接 进 行 物理 人 机 交互 的 阻抗 匹配 
策略 。 

物理 的 人 机 交互 (physical Human-Robot Interaction,pHRI) 旨 在 提高 机 器 人 系统 的 安全 
性 。 人 机 交互 控制 主要 有 两 种 方法 : 位 置 / 力 控制 和 阻抗 控制 。 与 位 置 / 力 控制 相 比 ,阻抗 控 
制 对 自由 运动 和 约束 运动 之 间 的 转换 具有 更 强 的 适应 性 。 当 外 部 约束 条 件 已 知 时 ,阻抗 控制 
有 较 好 的 跟踪 能 力 。 

通过 机 器 人 和 人 手 之 间 的 交互 力 来 控制 机 器 人 的 末端 执行 器 ,其 中 交互 力 采 用 力 传 感 器 
测量 。 本 节 将 阻抗 控制 应 用 于 人 机 交互 任务 。 基 于 阻抗 控制 的 人 机 交互 系统 有 效 性 依赖 于 人 
体 手 臂 的 行为 ,可 将 其 视 为 阻抗 模型 的 假设 。 交 互 系 统 的 操作 性 能 很 大 程度 上 取决 于 人 体 手 
臂 阻 抗 模型 的 准确 性 。 研 究 表明 ,根据 人 体 手臂 阻抗 的 变化 来 调节 机 器 人 阻抗 模型 的 参数 是 
提高 人 机 交互 性 能 的 有 效 方 法 。 人 体 手 臂 的 阻抗 参数 可 以 通过 相关 实验 来 识别 。 

人 体 手 臂 的 阻抗 参数 一 般 通过 对 人 体 手 臂 的 端点 施加 扰动 并 记录 恢复 力 曲线 来 估计 。 在 
实际 应 用 时 ,这 种 基于 扰动 的 方法 极 不 方便 。 于 是 ,有 人 提出 一 种 在 线 计 算 效率 较 高 的 人 手臂 
末端 笛 卡 儿 刚 度 估 计 模 型 ,该 模型 使 得 在 远程 阻抗 控制 应 用 中 描述 臂 端点 的 刚度 成 为 可 能 。 

本 节 采 用 人 体 手 臂 笛 卡 儿 空间 下 刚度 的 估计 模型 对 其 末端 刚度 进行 实时 估计 。 这 里 基于 
阻抗 参数 (质量 .阻尼 和 刚度 ) 来 完成 人 体 手 臂 的 阻抗 估计 模型 。 在 获得 人 体 手臂 的 阻抗 参数 
后 ,利用 LQR 定义 所 需 的 机 器 人 阻抗 模型 。 一 旦 产生 机 器 人 阻抗 模型 ,就 可 以 建立 机 械 臂 笛 
卡 儿 空间 中 位 移 和 力 之 间 的 期 望 关 系 。 因 此 ,机 器 人 的 期 望 关节 角度 可 以 通过 道 运 动 学 根据 
安装 在 机 械 末 端 执行 器 处 的 力 /力矩 传感器 测量 的 力 来 计算 。 由 于 机 器 人 系统 中 动态 参数 的 
不 确定 性 一 直 存 在 ,依靠 精确 机 器 人 动力 学 模型 的 控制 方法 已 经 难以 满足 期 望 的 控制 性 能 。 
因此 ,需要 引入 基于 函数 通 近 技术 (Function Approximation Technique,FAT) 的 自 适 应 控制 
器 来 控制 机 械 臂 末端 执行 器 的 位 置 ,并 且 可 以 通过 在 线 学 习 算 法 来 近似 机 器 人 和 人 体 手 臂 系 
统 的 动态 参数 ,以 确保 能 够 达到 期 望 的 控制 性 能 。 


13.1.1 人 机 交互 系统 


1. 人 机 交互 系统 的 结构 
基于 主 从 结构 的 阻抗 匹配 系统 框架 如 图 13. 1 所 示 。 表 面 肌 电 信号 和 手臂 姿态 信息 可 使 
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用 佩戴 在 人 体 上 臂 和 前 臂 上 的 两 个 MYO 手 环 来 测量 ,MYO 手 环 通过 无 线 网 络 连 接 到 主 计 
算 机 。 然 后 ,由 主 计算 机 发 出 控制 命令 并 通过 UDP 发 送 到 从 计算 机 (Linux/ROS) ,用 于 控制 
机 器 人 。 


BaxterIM 机 器 人 MYOTIM 肌 电 手 环 


LinuX/ROS 


图 13.1 人 机 交互 系统 的 示意 图 


MYO 手 环 有 八 个 相同 的 单元 ,每 个 单元 由 EMG 传感器 和 九 种 惯性 测量 单元 组 成 ,包含 
三 轴 陀 螺 仪 三 轴 加 速度 计 和 三 轴 磁 力 计 。 因 此 ,可 以 使 用 从 陀螺 仪 收集 的 上 臂 和 前 臂 的 方向 
言 息 来 收集 原始 EMG 数据 并 计算 臂 配置 。 

Baxter 机 器 人 的 每 个 机 械 臂 有 七 个 自由 度 (DOF) ,由 肩 关 节 中 的 三 个 自由 度 、 肘 关节 中 
的 一 个 自由 度 和 腕 关节 中 的 三 个 自由 度 组 成 ,并 且 可 以 在 力矩 或 位 置 模 式 下 控制 每 个 臂 关节 。 
前 面 章节 对 此 已 有 详细 介绍 ,这 里 不 再 歼 述 

2. 人 机 交互 原理 

人 机 交互 系统 包含 人 手臂 和 机 械 辟 ,如 图 13. 2 所 示 。 机 械 臂 末端 执行 器 上 安装 有 力 传 感 
器 。 这 里 ,使 用 MYO 手 环 获得 人 手臂 的 姿态 和 


肌肉 活动 水 平 , 利 用 力 传感器 测量 人 手 与 机 械 辟 MYOj 电 于 环 机 械 手 
端 执 行 器 之 间 的 相互 作用 力 下 。 力 传 三 
机 械 辟 运动 学 如 下 : 


工 二 p(9) LO 
其 中 ,xz ER” 和 gE€R" 分 别 是 笛 卡 儿 空 间 中 的 图 13.2 人 体 手臂 和 机 械 辟 交互 简化 模型 
末端 执行 器 位 置 和 关节 空间 中 的 角度 。n。 表示 笛 卡 儿 空 间 维 数 ,n 分 别 表示 机 械 臂 的 自 


由 度 。 
对 式 (13. 1) 求 关于 时 间 的 二 阶 导 数 如 下 : 
=Jr(g)g +Jr(g)Y (13. 2) 
其 中 ,Jk (gq)ER"” 表示 机 器 人 雅 可 比 和 矩阵 。 
机 械 臂 的 动力 学 描述 如 下 : 
Mr (do)5 十 CRCdoa) 十 G， =r— JgF CP 


其 中 ,Mk (q)ER” ”是 关节 空间 中 的 惯性 矩阵 ,CR (gq ,9)ER” 是 科 里 奥 利 矩 阵 ,G, (q) ER” 
表示 重力 项 ,rE€ R” 是 输入 控制 变量 。F € R” 表示 人体 手 臂 施 加 的 相互 作用 力 , 它 是 系统 的 
输入 。 

阻抗 控制 是 控制 机 械 辟 和 环境 (这 里 的 环境 是 指 人 的 手臂 ) 之 间 的 动态 相互 作用 (相互 作 
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用 力 和 轨迹 ) 的 常用 方法 。 机 械 辟 阻抗 控制 模型 如 图 13. 3 所 示 。 


机 械 臂 关节 


< 
pF 位 置 控制 器 1 


图 13.3 机 械 臂 阻抗 控制 模型 


笛 卡 儿 空间 中 机 器 人 阻抗 模型 如 下 : 
人 六 二 下 (13. 4) 
其 中 ,TI(。) 是 需要 计算 的 所 需 阻 抗 函数 ,zy 表示 机 械 臂 末端 执行 器 的 参考 位 置 ,z。， 为 机 械 
臂 未 端 执行 器 的 所 需 位 置 。 
阻抗 函数 1(，) 的 确定 依赖 于 人 体 手臂 阻抗 参数 。 交 互 过 程 中 人 体 手臂 的 动态 行为 可 描 
述 如 下 : 
了 让 十 万 H 之 十 下 了 三 一 下 (13..5) 


和 Kp ER””“ 分 别 表示 人 体 手 臂 的 质量 .阻尼 和 末端 笛 卡 


nxn 
< < 


其 中 ,TIER7 DER 
儿 刚 度 和 矩阵 。 


13.1.2 人 手臂 阻抗 参数 识别 

1. 人 手臂 雅 可 比 计算 

质量 .阻尼 和 刚度 项 将 随 着 人 手臂 状态 的 变化 而 变化 ,包括 手臂 姿态 .肌肉 激活 水 平等 。 对 
于 人 体 手臂 阻抗 参数 的 在 线 识别 ,这 里 采用 维 数 减 少 的 人 体 手臂 端点 刚度 表示 来 估计 Kj , 即 

Knp(p,q)=Jh (q)[Le(p)K; 一 GCq)]JT (9) (13. 6) 
其 中 ,Kj 是 人 类 手臂 的 笛 卡 儿 刚 度 矩 阵 , 它 是 肌肉 共 激活 指数 p 和 人 手臂 配置 参数 g 的 函 
数 。J 六 是 人 类 雅 可 比 矩 阵 ; 状 ) 是 一 个 常数 矩阵 ,反映 了 人 体 手 臂 在 最 小 肌肉 活动 时 的 关节 
时 2 
刚度 。eCp) 依 闲 于 肌肉 活动 的 指数 ,用 于 反映 刚度 分 布 大 小 的 调整 。GC4g) 一己， 一 , 它 反 
上 映 了 外 力 对 笛 卡 儿 空间 和 关节 空间 之 间 刚 度 变化 的 影响 。 式 (13. 6) 显 然 依赖 人 体 雅 可 比 Jp 
处 理 端点 刚度 分 布 的 几何 变化 。 

为 了 获得 人 体 手臂 的 雅 可 比 矩 阵 , 需 要 实时 跟踪 关节 位 置 /角度 。 可 以 通过 将 两 个 MYO 
手 环 内 的 陀螺 仪 获得 的 四 元 数 转换 为 人 体 手臂 运动 模型 中 的 关节 角度 来 实现 ,两 个 MYO 手 
环 代表 上 臂 和 前 臂 的 方向 。 

具有 代表 性 的 人 体 手 臂 模型 用 于 表示 简化 的 人 体 手臂 运动 学 ,如 图 13. 4 所 示 。 基 础 框架 
位 于 肩 部 的 中 心 ,z。 和 zx。 的 方向 分 别 被 认为 是 基础 框架 的 轴 , 水 平 向 上 和 水 平 向 右 。o， 是 
手掌 的 中 心 ,z， 是 手指 的 方向 ,zx; 是 手掌 的 法 线 向 量 的 方向 。 工 ,。、L,。 是 上 辟 和 前 臂 的 长 度 ， 
L， 是 从 手腕 中 心 到 手掌 中 心 的 长 度 。 . 

根据 对 人 体 手臂 的 研究 ,人 体 手臂 三 角 模型 用 于 表示 手臂 配置 。 因 为 三 角形 空间 和 关节 
空间 之 间 存 在 一 对 一 的 映射 关系 ,这 意味 着 人 类 手臂 三 角形 可 以 唯一 地 确定 手臂 配置 。 在 不 
考虑 手势 的 情况 下 ,人 体 三 角 模型 的 三 个 参数 (上 辟 的 方向 、 人 体 手臂 的 平面 方向 ,以 及 上 项 和 
前 臂 之 间 的 角度 ) 可 以 很 容易 地 从 上 臂 和 前 臂 的 方向 跟踪 四 元 数 。 图 13. 4 中 涉及 的 前 四 个 关 
节 角 度 可 以 通过 反 向 运动 学 (Inverse Kinematics,IK) 算 法 计算 ( 肩 部 的 前 三 个 关节 和 肘 部 的 
单个 关节 用 于 确定 人 体 手臂 终点 ) 。 图 13. 4 所 示 的 D-H 模型 可 用 于 计算 相应 的 人 体 雅 可 比 
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行列 式 Jr 。 


图 13.4 人 体 手 臂 D-H 模型 


2. 最 小 活动 臂 关 节 刚 度 和 共 激 活 指 数 识别 

通过 经 典 扰动 方法 可 以 识别 最 小 关节 刚度 KK， 和 表达 式 C(p) 中 未 改变 的 参数 。Baxter 
机 器 人 的 一 个 夹具 夹 住 受 试 者 的 腕 关节 以 施加 随机 扰动 。 为 这 些 扰动 实验 选择 5 个 不 同 的 臂 
端点 位 置 。 对 于 每 个 位 置 , 肘 关节 升 高 或 降低 以 形成 3 个 不 同 的 高 度 , 同 时 保持 端点 位 置 不 
变 。 因 此 ,总 共 可 以 获得 15 种 不 同 的 配置 。 对 于 每 种 配置 ,指示 受 试 者 尽 可 能 保持 3 种 不 同 
的 肌肉 共 激 活水 平 (小 活动 、 中 活动 和 高 活动 )。 实 验 中 ,反映 共同 激活 水 平 的 经 处 理 的 EMG 
信号 将 显示 在 受 试 者 前 面 的 屏幕 上 以 帮助 实现 该 目的 。 在 这 些 扰动 实验 期 间 , 原 始 EMG 信 
号 和 人 体 手臂 配置 可 以 通过 两 个 MYO 手 环 获得 。 
由 于 上 和 臂 和 前 臂 的 长 度 是 已 知 的 ,因此 可 以 跟踪 人 
体 手 臂 的 端点 位 置 (手腕 的 中 心 ), 并 通过 安装 在 机 
械 臂 末端 执行 器 上 的 力 传 感 顺 记录 恢复 力 , 实 验 装 
置 如 图 13.5 所 示 。 

肌肉 共 激 活 指数 p 可 通过 以 下 步骤 获得 ( 见 
图 13.6): 采用 移动 平均 过 程 和 低 通 滤波 器 从 
EMG 信号 中 提取 包 络 。 尽 管 MYO 手 环 内 的 
EMG 传感器 有 8 个 通道 ,但 只 需要 用 其 中 两 个 靠 
近 对 抗 肌肉 的 通道 一 一 二 头 肌 和 三 头 肌 。 因 此 , 肌 
肉 共 激活 水 平 可 以 用 下 面 的 等 式 表 示 ; 


图 13.5 用 于 终点 刚度 校准 的 实验 装置 


W.—1 W, 一 1 
ph) = (DEG —k)+ 六 Er 一 有 全 35 
5 R 王 1 k=1 
其 中 ,W, 是 预 设 窗口 大 小 ,Ep(，) 和 Er(，) 是 Biceps 和 Triceps 的 包 络 EMG 信号 的 幅度 ， 
上 和 分 别 表 示 当 前 采样 时 间 和 采样 点 。 


机 械 臂 端点 位 移 和 恢复 力 之 间 的 动态 关系 可 以 描述 如 下 : 
| pl Oe | A 
R'is Gs Gu oy [eA (13. 8) 
沿 GO， | A 
其 中 ,F,. \.F ,和 下 .。 是 末端 恢复 力 ,Ax.、Ay。 和 Az. 是 笛 卡 儿 坐 标 系 中 的 末端 位 移 。 
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通常 ,可 以 利用 二 阶 线性 模型 来 识别 每 个 单 输入 单 输 出 传递 函数 G; 。 
Gs = Ts Days Ks =2xf VM—1 (13.9) 
传递 函数 G 的 参数 Tn .Dp 和 Kp 可 以 利用 0 一 20Hz 的 最 小 二 乘法 来 识别 。 
因此 ,通过 肌肉 最 小 共 激 活水 平 的 试验 确定 的 所 有 Kp 可 用 于 通过 最 小 化 下 面 的 Frobenius 
范 数 来 识别 恒定 的 最 小 关节 刚度 Kj: 


IKj,—Jh(Ky(p,q)Jn(q)—G(g) | (13. 10) 
此 外 ,通过 最 小 化 下 面 的 Frobenius 范 数 ,利用 Kp 来 识别 参数 c(p)、Yl 和 y，: 
| eCpOKj;—Jh(gq Ka(p,q nq) — Gq) | CS TT 
niLl—e 
相生 18. 12) 
le ’ 


由 于 可 以 获得 最 小 活动 的 臂 雅 可 比 矩 阵 、 共 激活 指数 和 关节 刚度 ,所 以 可 通过 式 (13. 6) 所 
示 的 在 线 跟踪 臂 端 点 刚度 Ky。 忽略 肌肉 质量 分 布 对 预定 义 姿势 附近 I 的 影响 CTr 在 以 下 
讨论 中 被 假定 为 常数 )。 根据 对 实验 结果 的 观察 和 分 析 , 发 现 阻尼 和 矩阵 的 变化 在 以 每 个 水 平 
(最 小 、 中 、 高 ) 为 中 心 的 肌肉 激活 水 平 的 小 范围 内 不 明显 。 因 此 ,我 们 假设 阻尼 矩阵 的 轨迹 可 
以 离散 化 为 3 个 不 同 的 矩阵 ,对 应 于 每 个 臂 配置 的 3 个 不 同 的 肌肉 激活 水 平 。 因 此 ,可 以 根据 
实验 数据 在 合理 范围 内 建立 阻尼 矩阵、 人 体 手 臂 配 置 和 肌肉 激活 水 平 之 间 的 查找 表 。 
一 脏 二 头 肌 EMG 信 和 号 


一 一 肪 三 头 肌 EMG 信 号 
个 包 络 线 


中 激活 水 平 


最 大 激活 


ial, 


2 3 Ee: 6 7 


4 
时 间 /s 
图 13.6 通过 移动 平均 过 程 和 低 通 滤波 器 获得 的 肌肉 共 激 活 指数 p 的 包 络 线 


13.1.3 机 器 人 阻抗 控制 模型 
考虑 如 下 时 不 变 线性 系统 : 
二 An(t) + Bult) (13. 13) 
其 中 ,mE R? 表示 机 械 臂 系统 的 状态 ,wu ER” 表 示 系 统 的 输入 ,矩阵 AER??* 和 BER?** 已 
知 。 机 械 臂 系统 7 的 状态 定义 如 下 : 
7= [2 zxz2 7 (13. 14) 
其 中 ,oc ER” 是 线性 系统 的 状态 ,用 于 确定 机 械 臂 末端 执行 器 的 参考 位 置 Zr 并 且 提 供 了 实 
现 其 优化 轨迹 的 跟踪 控制 的 可 行 性 。 系统 如 下 : 
6=V—o 
(13. 15) 
AW 
是 已 知 矩阵 。 考 虑 到 定义 的 状态 式 (13.5) ,可 以 在 式 (13. 13) 中 


7 Xn 
< < 


其 中 ,VER” 和 全 ,ER 
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定义 矩阵 4 和 B 如下: 


一 ID ,.— 1KRyg .0 一 JJ 如 
A= | 0 0|,“ B=i| 6 vi WG Le 
0 0 YY 0 
设计 如 下 线性 二 次 型 调节 器 以 最 小 化 代价 函数 
r=| [zl0,z, Frm) Oy) TRu ld (13.17) 
其 中 ,OER” ,0Q, ER 和 RER"” 分 别 是 速度 、 轨 迹 跟 踪 误 差 和 系统 输入 的 加 权 


和 矩阵。 此 外 ,Q， 和 Q, 是 对 称 的 、 半 正定 的 ,R 对 称 且 正定 。 
根据 式 (13. 14) . 式 (13. 15) 和 式 (13. 17) 将 代价 函数 写成 如 下 形式 : 


r=| [7'Q7 +u Rud (13. 18) 


Qi 0 0 
其 中 ,Q@= | 0 2， a 
0 一 VIO，VIO,Y 
线性 二 次 型 控制 器 主要 是 用 来 设计 一 个 最 佳 的 反馈 控制 器 ,使 得 代价 函数 式 (13. 18) 的 值 
最 小 。 则 机 械 臂 系统 输入 x 可 以 定义 如 下 : 
& =—Kin (13. 19) 
其 中 ,Ki 是 状态 反馈 控制 器 的 增益 矩阵 ,定义 为 
天 ER 二 PP (13. 20) 
其 中 ,PER2>? 是 代数 Riccati 方程 的 解 。 
考虑 式 (13. 19) ,将 人 手 施加 的 相互 作用 力 (F ) 作 为 机 械 辟 阻抗 模型 的 系统 输入 wu。 根 据 
式 (13. 19) \ 式 (13. 20) 和 Riccati 等 式 ,期 望 的 阻抗 模型 可 写成 如 下 形式 : 
F=—Kiy=—R 'B'K, 
=—R™ Pnis—R Piyzr SR PitViV) zy CE 1 
其 中 ,Pl1 ER "“ .Pls ER”"” 和 Pl;ER”"“~”" 是 块 矩阵 己 的 第 一 行 中 的 3 个 子 和 矩阵 (PE 
R" “)。 根 据 外 力 下 和 预 设 轨迹 zy ,获得 机 械 臂 末端 执行 器 所 需 轨迹 zx 。 此 外 ,可 以 采用 
反 向 运动 学 来 获取 机 械 辟 的 相应 期 望 关节 角度 qi 。 需 要 一 个 位 置 控制 器 来 确保 所 需 关 节 角 
度 的 跟踪 性 能 。 
13.1.4 基于 函数 逼近 的 自 适 应 控制 器 
采用 基于 函数 逼近 技术 的 自 适 应 控制 器 来 控制 机 器 人 在 关节 空间 中 的 轨迹 。 为 了 实现 机 
械 臂 的 控制 算法 ,定义 如 下 参数 : 


下 = 
Se CEO Za) 
ty es 
A a 
由 式 (13. 3) 和 式 (13. 22) 可 得 
Mr (gq)s+Mr(g)V+CR(g 9)s + CR(qg IU +GRg9) =r— TAR)F (13.23) 
因此 , 自 适 应 控制 器 可 以 为 
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tr=Mr (gy HOR (gv HOR (Cg) +IR GFK,s :C19 24) 


根据 式 (13. 23) 和 式 (13. 24) ,可 得 闭环 关节 轨迹 控制 系统 : 


Mr(g); +Cr(g,9)s — K,s =— (Mr(g) —Mr(g))o — 
(CR (go) — Cgr(g,9)v— (Gr(g) —Gr (gq)) (13. 25) 
显然 , 式 (13.25) 将 以 适当 的 增益 矩阵 K, 接近 目标 表达 式 。 采 用 如 下 函数 近似 方法 : 
MR (9g) =Wh, Zu,, Cr(q;9)=We Ze,, Gr(g)=W6,Zo, (13..26) 
其 中 ,Wx ER 和 Wo ER" "是 惯性 离心 力 、 科 里 奥 利 力 和 重力 的 权 
重 ,。 ZN ER Ze ER “和 Ze 三 浊 生 天 泛 畴 届 数 答 降 5 
Mr (9g)、Cr(g,9) 和 Gr (gq) 的 估计 值 如 下 : 
2 (gy Wh Zi 


Cr (qd) =WE Ze C13. 27) 


nxXn 
< < 


‘We, ER 


n. Xn. 
< < 


Cn (gq) =WE Zo 


更 新 规则 为 
Wa = —Q wm Zn 
属 =—Qe Zc os (LS. 28) 
Woe, =—Q6Zo.s 
采用 以 下 李 雅 普 诺 夫 函 数 


1 ee > < 要 
V = + Ftr Wu Qu Wu HWE Qc We, HW6 Qo Wo,) (13.29) 


其 中 ,Qu “Qc, 和 Qc 为 正定 权重 矩阵 。 ir-() 表 示 和 矩阵 的 迹 , 而 Wy. =Wm. —Wy, ,We 3 
Wo —We, 和 Wo, Won We. 。 

综 上 可 得 

V=—s'K,s (13. 30) 

容易 证 明 ,机 械 辟 系统 是 稳定 的 ,因为 V 是 负 定 矩阵 , 当 t~>~co 时 ,0, 则 9 一 qv 一 ce。 通过 
这 种 方式 ,实现 了 人 体 手臂 和 机 械 臂 的 最 优 交 互 。 

13.1.5 仿真 

1. 仿真 参数 设置 

仿真 时 , 先 用 机 器 人 工具 箱 建立 一 个 两 自由 度 模拟 机 械 臂 和 一 个 四 自由 度 模 拟 机 械 臂 ,分 
别 模拟 机 械 臂 和 人 体 手 臂 , 以 验证 上 述 方法 。 

模拟 机 械 臂 的 参数 如 表 13. 1 所 示 , 其 中 办 和 7 分 别 表 示 质 量 .长 度 和 惯性 参数 。 假 设 
人 体 手 臂 产 生 的 外 力 仅 在 X 方向 上 ,并 且 不 考虑 Y 和 Z 方向 的 力 。 机 械 臂 的 初始 配置 预 设 为 
di 一 r/3,9q2 一 一 (2r)/3。 因 此 ,模拟 机 械 臂 的 初始 端点 位 置 为 zxr 一 0. 4m。 参 考 端点 轨迹 由 
式 (13.15) 确 定 , 其 中 V 二 1,W 二 0.5。 因 此 ,参考 端点 轨迹 为 zj 一 0.5 一 0. le“, 其 目标 位 置 
为 zj=0.5m(t 一 00)。 
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表 13.1 机 械 臂 模型 参数 
连 杆 1 的 质量 
连 杆 2 的 惯性 


连 杆 1 的 长 度 
连 杆 2 的 长 度 


1.00kg 
0.001kg* m’ 
0.001kg* m’ 


由 于 矩阵 4 和 B 可 以 在 获得 人 体 手臂 阻抗 模型 的 基础 上 计算 ,根据 式 (13. 21), 可 得 
Riccati 方程 的 解 ,从 而 可 得 所 需 的 机 器 人 阻抗 模型 。 

2. 不 同 臂 配置 和 模拟 结果 活动 水 平 

考虑 两 种 不 同 的 肌肉 活动 水 平 , 人 体 手 臂 配 置 如 图 13.7 所 示 。LQR 方法 Q 和 R 的 系数 
预定 义 为 Q1==1,Q, = 二 1 且 R=1。 


图 13.7 两 种 不 同 的 人 体 手臂 姿态 


不 同 的 手臂 配置 和 肌肉 活动 水 平 导 致 人 体 手臂 的 刚度 不 同 。 因 此 ,根据 可 变 的 人 体 手臂 
端点 阻抗 参数 来 调整 相应 的 机 器 人 阻抗 模型 。 在 LQR 方法 下 机 械 辟 轨迹 以 及 机 械 臂 与 人 体 
手臂 之 间 的 交互 力 如 图 13. 8 一 图 13. 13 所 示 。 


时 间 /s 
图 13.8 机 械 辟 端点 轨迹 : 手 辟 姿态 ,调整 参数 c(p)==1.55 


图 13.7(a) 中 臂 端点 的 轨迹 和 力 如 图 13. 8 一 图 13. 11 所 示 , 图 13.7(b) 中 辟 端 点 的 轨迹 和 
力 如 图 13. 12 和 图 13. 13 所 示 。 图 13. 8 一 图 13. 11 表示 不 同 肌 肉 活动 水 平 的 影响 ,端点 刚度 
增加 对 应 于 扩大 的 c(p)。 图 13.8、 图 13.9、 图 13. 12 和 图 13. 13 表示 不 同 臂 形 的 影响 ,因为 
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0.06 Kuy=0.53kN/m 
0.04 Kiy=0.62kN/m 
Kis=0.20kN/m 


相互 作用 力作 


| -A 0 


5 
时 间 /s 
图 13.9 相互 作用 力 : 调整 参数 c(p)=1.55 


上 
| 
| 
时 间 /s 
图 13.10 机 械 辟 端点 轨迹 : 手臂 姿态 ,调整 参数 c(p)==2.30 
0.06 KHx=0.90kN/m 
区 0.04 KHv=1.15kN/m 
SEO KHz=0.43kN/m 
时 
A 
ey 
至 
时 间 /s 
图 13. 11 相互 作用 力 : 调整 参数 c(p)==2. 30 
上 
了 
| 


4 
一 时 间 /s 


i ni 


图 13.12 机 械 辟 端点 轨迹 : 手臂 姿态 ,调整 参数 c( 妃 ) 一 1.53 


只 考虑 了 X 方向 的 力 , 图 13.7(b) 中 臂 形 的 X 方向 上 的 端点 刚度 大 于 图 13. 7(a) 臂 形 的 端点 
刚度 ,因此 ,在 稳定 的 相互 作用 力 下 ,c(p)==1. 53 时 ,图 13.7(a) 所 示 臂 形 的 位 移 实 际 上 更 接 
近 于 c(z) 王 2. 30 时 图 13.7(a) 所 示 臂 形 (a) 的 位 移 。 
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0.06 Kx=0.85kN/m 
0.04 Ky=0.32kN/m 
0.02 Ks=0.28KN/m 


| 时 间 /s 
图 13.13 相互 作用 力 : 调整 参数 c(p)==1.53 


13.2 基于 视觉 交互 和 RBF 神经 网 络 的 机 器 人 遥 操 作 系统 


本 节 将 介绍 一 种 机 器 人 遥 操 作 示 教 方法 ,该 方法 使 用 基于 视觉 交互 的 虚拟 遥 操 作 系 统 和 
基于 RBF 神经 网 络 的 学 习 方 法 。 

近年 来 , 随 着 机 器 人 技术 的 飞速 发 展 , 机 器 人 在 工业 中 的 应 用 已 扩展 到 各 个 领域 。 通 过 示 
教 (Teaching by Demonstration， TbD) 方 法 ,机 器 人 可 以 在 新 的 工作 环境 中 执行 不 同 于 以 往 
的 任务 。 传 统 上 ,只 有 在 专业 人 员 花 费 大 量 时 间 通 过 键盘 或 操纵 杆 编程 之 后 ,工业 机 器 人 才能 
在 装配 线 上 学 习 固 定 的 技能 。 显 然 , 对 于 现代 制造 业 ,这 种 方法 通常 费时 且 不 灵活 。 而 机 器 人 
通过 遥 操 作 从 熟练 的 演示 者 那里 学 习 类 似 人 类 的 操作 技能 ,从 而 直接 编程 ,这 种 方法 可 以 使 机 
器 人 有 效 地 适应 不 同 的 任务 或 环境 。 与 传统 的 机 器 人 示 教 方法 相 比 ,机 器 人 通过 遥 操 作 进 行 
示 教 能 够 更 有 效 、 更 自然 地 学 习 各 种 类 人 技能 。 由 于 基于 人 机 交互 (Human Robot 
Interaction，HRI) 的 遥 操 作 具 有 上 述 优点 ,近年 来 受到 了 广泛 的 关注 。 

有 许多 技术 或 设备 应 用 于 HRI, 一 般 来 说 ,视觉 交互 是 应 用 最 广泛 的 技术 之 一 。 由 于 基 
于 身体 运动 跟踪 的 视觉 交互 相对 容易 实现 ,所 以 大 部 分 都 用 于 捕 抱 人 体 运 动 。 而 在 机 器 人 
TbD 方法 中 ,神经 网 络 也 得 到 了 广泛 的 应 用 。 例 如 ,建立 自 适 应 控制 系统 的 TbD 方法 ,在 神 
经 网 络 的 帮助 下 ,通过 重复 一 项 任务 来 提高 机 器 人 的 工作 性 能 。 也 有 人 将 神经 学 习 用 于 估计 
稳定 动力 系统 , 且 实验 结果 表明 ,该 方法 能 够 准确 地 对 系统 进行 评价 。 

本 节 将 介绍 一 种 基于 视觉 交互 的 虚拟 遥 操 作 系 统 的 机 器 人 示 教 方法 ,以 及 基于 (Extreme 
Learning Machine，ELM) 的 神经 学 习 方 法 。 更 具体 地 说 ,以 Baxter 机 器 人 为 研究 对 象 ,使 用 
动作 捕捉 设备 Kinect V2 来 跟踪 人 体 运 动 状态 ,并 在 V-REP 平台 上 进行 仿真 实验 。 此 外 ,本 
节 还 基于 RBF 神经 网 络 学 习 算 法 , 教 机 器 人 学 习 人 类 的 技能 。 与 使 用 其 他 神经 网 络 的 机 器 人 
示 教 相 比 , 该 模型 训练 样本 较 少 , 泛 化 能 力 强 。 


13.2.1 机 器 人 遥 操 作 系 统 

本 节 设 计 的 系统 包括 一 个 虚拟 遥 操 作 系统 和 一 个 训练 学 习 系统 , 如 图 13. 14 所 示 。 第 一 
阶段 ,人 类 示范 者 通过 Kinect 传感器 控制 机 器 人 仿真 软件 V-REP 中 的 Baxter 机 器 人 。 在 第 
二 阶段 ,将 使 用 神经 网 络 对 数据 进行 训练 和 学 习 , 并 在 第 一 阶段 进行 记录 。 然 后 将 输出 数据 发 
送 给 Baxter 机 器 人 ,控制 它 完成 前 面 的 任务 。 虚 拟 遥 操作 系统 是 对 真实 遥 操 作 系统 的 仿真 ， 
在 虚拟 环境 中 可 以 验证 上 述 算法 。 虚 拟 系统 模型 逼真 度 较 高 ,能 够 让 人 类 示范 者 获得 比较 真 
实 的 交互 体验 。 

这 里 介绍 的 基于 HRI 的 虚拟 和 遥 操作 系统 由 人 类 示范 者 .Kinect 传感器 和 安装 有 V-REP 
软件 的 计算 机 组 成 。 其 中 ,Kinect 传感器 用 来 捕捉 人 体 动 作 以 及 人 手 的 状态 (如 打开 、 握 拳 
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je 


Kinect V2 1 
加 时时 一 > 在 信 区 


的 Baxter 机 器 人 神经 网 络 
图 13.14 机 器 人 示 教 系统 


-等 ); 虚拟 的 Baxter 机 器 人 模型 和 环境 则 是 由 V-REP 软件 提供 。 该 各 操作 系统 的 主要 控制 
目标 是 让 V-REP 软件 中 Baxter 机 器 人 的 机 械 臂 能 够 跟随 人 类 示范 者 的 运动 。 

1. Kinect 传感器 

这 里 使 用 第 二 代 Windows Kinect( 如 图 13. 15 所 示 ), 它 由 一 个 RGB 彩色 摄像 头 一 个 深 
度 传 感 器 和 一 个 红外 发 射 器 构成 ,其 中 深度 传感器 
由 一 个 红外 摄像 头 和 红外 投影 仪 构 成 。 通 过 以 上 
这 些 设备 ,Kinect 传感器 可 以 提供 全 身 三 维 运 动 捕 
提 、 人 脸 识 别 等 功能 。 与 Kinect V1( 请 参考 10. 3. 3 
节 ) 相 比 ,Kinect V2 可 以 跟踪 人 体 的 25 个 身体 关 ”RGB 摄像 关 ”深度 摄像 头 红外 发 射 器 
节 ( 包 括 拳 头 和 拇指 )。 由 于 这 一 优势 ,Kinect V2 图 13. 15 ”Kinect V2 传感器 的 构成 
可 以 识别 手 部 的 状态 。 如 打开 .关闭 和 套 索 。 套 索 
状态 是 指 手 的 中 指 和 食指 伸展 ,其 他 手指 闭 上 的 手势 。 手 部 的 状态 是 用 来 控制 机 器 人 是 否 跟 
随 人 类 示范 者 的 动作 。 

2. V-REP 

V-REP 是 一 个 具有 集成 开发 环境 的 开源 机 器 人 仿真 器 。V-REP 中 机 器 人 模型 的 构建 和 
仿真 过 程 如 下 。 

(1) V-REP 场景 中 建立 机 器 人 模型 和 工作 环境 : 构建 机 器 人 仿真 模型 有 两 种 方法 ,其 中 
一 种 方法 是 先 在 其 他 软件 中 创建 模型 ,并 将 其 保存 为 某 种 特定 的 文件 格式 ,如 OBJ、3DS 和 
URDF 等 ,最 后 再 将 保存 的 文件 导入 V-REP。 另 一 种 方法 是 直接 从 模型 浏览 器 中 加 载 现 有 
模型 。 

(2) 使 用 编程 方法 控制 对 象 / 模 型 : V-REP 有 6 种 编程 方法 来 控制 每 个 模型 ,分 别 为 租 入 
式 脚本 、 附 加 组 件 插件、 远程 API 客户 端 \ROS 节点 和 自 定 义 客户 端 /服务 需 。 

(3) 选择 物理 引擎 并 设置 模拟 参数 : V-REP 提供 Bullet Physics、ODE、Vortex Dynamics 
和 Newton Dynamics 4 种 物理 引擎 来 模拟 真实 物体 间 的 相互 作用 。 这 些 物理 引擎 可 以 使 动态 
计算 更 快 、 更 容易 。 

这 里 使 用 Baxter 机 器 人 模型 进行 仿真 ,并 基于 两 个 API 客户 端 来 控制 V-REP 中 的 
Baxter 机 妖 人 。 

3. Baxter 机 器 人 

Baxter 机 器 人 结构 如 图 13. 16 所 示 ,V-REP 中 的 Baxter 机 器 人 除了 头 部 和 动画 人 脸 外 ， 
与 真实 的 Baxter 机 器 人 相似 ,它们 有 相同 的 运动 学 和 动力 学 。 

4. Kinect 与 V-REP 之 间 的 通信 

连接 Kinect V2 ,需要 使 用 具有 USB 3. 0 端口 的 计算 机 。 虚 拟 遥 操作 系统 中 ,客户 端 为 用 
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图 13.16 真实 的 Baxter 机 器 人 与 V-REP 中 的 Baxter 机 器 人 


户 自 定义 的 C++ 应 用 程序 ,而 服务 器 端 则 为 V-REP 场景 。 要 在 客户 端 启用 远程 的 APL， 
V-REP 提供 的 C 语言 文件 应 包含 在 C++ 的 项 目 中 ,如 extApi.h、extApi.c、extApiPlatform.h 
和 extApiPlatform.c 等 。 要 在 服务 器 端 启用 远程 API, 应 在 启动 V-REP 时 加 载 远 程 API 插 
件 。 可 以 调用 阻塞 函数 获取 V-REP 中 Baxter 每 个 机 械 臂 的 关节 角度 , 即 客 户 端 可 以 获取 
V-REP 中 机 器 人 的 关节 信息 。 而 调用 非 阻 塞 函 数 可 以 将 关节 角度 发 送 给 V-REP 中 的 Baxter 
机 器 人 。 这 里 ,C++ 应 用 程序 用 来 记录 关节 角度 数据 并 将 其 保存 。 

13.2.2 还 操 作 系 统 中 关键 算法 

1. 空间 向 量 法 

如 何 计算 人 体 关节 角度 是 利用 Kinect 控制 Baxter 机 器 人 的 关键 ，Kinect 可 以 获取 人 体 
关节 的 笛 卡 儿 空间 坐标 。 假 设 笛 卡 儿 空 间 中 有 两 个 点 A 二 (xi,y1,z1) 和 B= (x,,y,,zs)， 
那么 可 以 通过 下 面 的 公式 计算 这 两 点 之 间 的 距离 : 

d=V(zxs—zi) + ys 一人) 十 (zy 一) (T3315 

则 向 量 AB= (zj 一 zyyy1 一 ys,z1 一 22), 点 A 和 B 之 间 的 距离 可 以 表示 为 d= 二 1AB|。 我 们 
知道 , 笛 卡 儿 空 间 中 可 以 使 用 余弦 定律 计算 两 关节 之 间 的 夹 角 。 在 Kinect 坐标 系 中 ,可 以 把 
每 一 个 人 体 关节 表示 成 一 个 向 量 。 设 关节 1 为 向 量 AB, 关 节 2 为 向 量 BC, 则 两 关节 之 间 的 


角度 可 以 表示 为 
EE (13. 32) 
|AB |.|BC | 
通过 式 (13. 31) 和 式 (13. 32) 可 以 将 由 Kinect 获取 的 人 体 关节 点 坐标 转换 成 对 应 的 向 量 , 并 计 
算出 各 关节 之 间 的 夹 角 。 


将 从 Kinect 获取 的 所 有 关节 点 的 位 置 坐标 构建 成 人 体 的 几何 模型 以 及 人 体 左 辟 模 型 ,如 
图 13. 17 所 示 。 在 Kinect 的 笛 卡 儿 空间 坐标 系 中 ,直线 OX .OY 和 OZ 组 成 空间 坐标 系 。 可 


以 通过 三 个 点 得 到 向 量 OF 和 向 量 EF ,计算 肩 距 角 和 角度 。 使 用 相同 的 方 可 得 肘 间距 
FEFG。 将 点 D.O 和 下 映射 到 平面 XOZ ,可 计算 出 户 部 偏 航 角 和 KOJ。 


使 用 向 量 乘法 ,可 得 向 量 LE 和 向 量 ME ,具体 计算 如 下 : 
LE =EF XPG 
ME =EF xDE 
因此 ,可 以 计算 得 到 肩 部 滚动 角 人 LEM。 同 理 ,可 计算 出 肘 部 滚动 角 ( 向 量 LE 和 向 量 


(13; 33) 
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图 13.17 由 Kinect 获得 的 人 体 骨 骼 和 人 体 左 臂 的 几何 模型 


GN 之 间 的 角度 ) 和 手 部 偏 航 角 ( 向 量 GN 和 G6 之 间 的 角度 ) 。 
2. 动态 时 间 归 整 法 (DTW) 
DTW 是 一 种 处 理 时 间 序 列 信号 的 算法 , 它 不 仅 可 以 比较 两 个 长 度 不 同 的 时 间 序 列 的 相 
似 性 ,而 且 可 以 通过 动态 规划 对 时 间 序 列 的 时 间 轴 进行 对 齐 。 
对 于 两 段 长 度 不 同 的 时 间 序 列 xz[zxi ,zs，… zj] 和 >yLyiyyz,， ,yn], 假 设 和 矩阵 WP 是 
由 DTW 建立 的 两 序列 间 的 匹配 路 径 , 表 示 如 下 : 
wp = 8 0 2 wo 
wpl» bz *** wpls 
其 中 民 为 WP 的 长 度 ,wppj E{l,2,…,m} ,wprsE{1;2,…,n},kE{1,2,…,L}。 在 WP 的 
约束 下 ,zx 和 y 之 间 的 距离 表示 如 下 : 


(13. 34) 


L 
pWP x59) SD Bp yw) (13. 35) 
k=1 


其 中 ,[zp ,wpiz]' 为 WP 的 第 上 列 , 且 WP 必须 满足 下 述 条 件 : 
(1) 边界 条 件 : [wp ,wpizs]==[1,1j 和 [wpLi,wprzj]==[m ,nj。 
(2) 单调 性 条 件 : 满足 wp 三 wps 二 … 二 wp 和 bip 委 zubz 委 … 委 wbiz 一 1， 
» ys 
(3) 步 长 条 件 : 满足 wpi 41 一 wp 人 1 和 wpi41,s 一 wppz 人 1,k 二 1,2,…,L 一 1。 
满足 上 述 条 件 的 路 径 有 很 多 ,而 DTW 的 目标 就 是 找到 最 佳 的 匹配 路 径 ,具体 表示 为 
DTW(zx,y)=min(p (WP ,zy)) (13: 36) 
动态 规划 解决 动态 时 间 规 整 问题 时 ,构建 了 一 个 m xn 维 的 累积 代价 和 矩阵 R。R(i,j) 是 
矩阵 R 的 元 素 , 表 示 如 下 : 


0 (C=17 1 
RUS Sg i 9 = 
R(i,7) (i>1,;=1) 
min(R(i 一 1,7j),R(i,j 一 1),R(i 一 1,; 一 1)) (其 他 ) 
(13537) 


使 用 DTW 处 理 机 器 人 示 教 时 关节 角度 的 样本 ,可 以 使 得 不 同 的 样本 在 时 间 轴 上 能 够 对 
齐 , 如 图 13. 18 所 示 。 
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0.2 入 1 
0.1 一 一 样本 2 
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时 间 / 步 时 间 / 步 
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图 13.18 (a) 关 节 角 度 S0 的 两 个 样本 和 (b) 两 样本 值 由 DTW 对 齐 


3. 基于 RBF 神经 网 络 的 示 教 
基于 仿真 愧 操作 系统 ,可 以 控制 Baxter 机 器 人 关节 角 的 运动 ,同时 还 可 以 用 一 阶 自治 常 
微分 方程 表示 为 这 些 点 -点 之 间 的 运动 如 下 : 
之 二 f(x) 十 e (13. 38) 
其 中 ,zx 表示 机 器 人 末端 执行 器 位 置 /关节 角度 ,是 x 关于 时 间 的 一 阶 导数 ,表示 机 器 人 末端 


执行 器 的 速度 /关节 角速度 。 数 据 集 为 {z+ ,二 ),““' ,e 是 零 均 值 高 斯 噪声 。 示 教 的 主要 任务 


是 从 了 获得 的 估计 。 
这 里 使 用 RBF 神经 网 络 的 方法 来 实现 。 可 以 通过 选择 合适 的 基 函 数 ,使 RBF 神经 网 络 
近似 任何 连续 函数 。RBF 神经 网 络 基于 数据 集 输入 层 隐 含 屋 输出 层 


(全 }2 人 学 习 二 和 之 之 间 的 映射 关系 , 即 
f: XX。 图 13. 19 所 示 的 RBF 神经 网 络 具 有 
隐藏 层 , 该 网 络 输入 层 有 7 个 节点 和 个 维度 。 
隐藏 层 中 目标 郴 数 可 以 表示 成 如 下 形式 : 
RW SR "(1839) 
其 中 ,Xi, E Qx CR" 表示 输入 向 量 , 广 为 RBF 


神经 网 络 的 输出 。 
儿 为 可 调节 的 参数 向 量 ,表示 如 下 : 
多 ==[ 如 | ;Ws (13. 40) 
S(X;, ) 是 输入 的 非 线 性 向 量 函 数 ,可 表示 为 
eb GS. et EC Gy (13. 41) 
式 (13.41) 中 ,S(X;,) 由 基 轴 数 ;; (Xio) 组 成 。 这 里 , 基 函 数 为 高 斯 郴 数 , 具 体 表 示 如 下 : 
(于 ;一 c))T 
S( 人 下 in ) 一 exp 一 (13. 42) 


其 中 ,ec; 王 [ci ?Ci2 人 为 向 量 , 它 表示 第 个 基 她 数 的 中 心 ,r 为 方差 。 
RBF 神经 网 络 可 将 函数 f 近似 为 任意 精度 ,表达 式 如 下 : 
f (Xi) 一 CXW )+e(Xi), Xn, E Ox， (13. 43) 
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式 中 ,e(XX;,) 为 近似 误差 ,W ”为 RBF 网 络 的 理想 权重 ,表示 如 下 : . 
W” =argminw Lsupx, en. | f (Xi,)—f (Xi,,W) | C13..44) 


近似 误差 受 正则 化 的 约束 ,可 以 表示 为 
| eXii) |<e , VX € OQx. 加 3 5 


神经 网 络 的 参数 主要 包括 RBF 函数 8 的 中 心 、RBF 函数 o 的 方差 和 权重 W。 通 常 ,6 是 
从 样本 中 选择 ,o 的 表达 式 如 下 
= (13, 46) 
V2M 
其 中 ,d,s 是 所 选 数据 中 心 之 间 的 最 大 距离 ,M 是 数据 中 心 的 个 数 。 
根据 最 小 二 乘 算法 ,权重 W 可 表示 为 
W=®@ d (13.472 


其 中 ,®@ ”是 @ 的 伪 道 ,可 以 表示 为 
B=B GB) BD! (13. 48) 
13.2.3 仿真 实验 
1. 虚拟 遥 操 作 系 统 有 效 性 验证 
首先 ,验证 通过 人 体 动作 控制 V-REP 中 的 Baxter 机 器 人 在 虚拟 空间 中 移动 的 灵活 性 。 
Baxter 机 械 臂 可 以 由 人 体 动作 控制 上 下 ,来 回 摆动 , 如 图 13. 20 所 示 。 需 要 注意 的 是 ,人 的 左 
rmi fe We De 实验 结果 表明 ,人 体 动作 可 


图 13.20 人体 运动 用 于 控制 V-REP 中 的 Baxter 机 器 人 


接 下 来 ,验证 机 器 人 能 和 否 通过 基于 视觉 交互 和 RBF 神经 网 络 的 遥 操 作 完 成 教 示 教 任务 。 
为 了 验证 TbD 方法 的 有 效 性 ,在 V-REP 中 设计 了 仿真 场景 ,场景 由 一 个 Baxter 机 器 人 ,一 张 
桌子 和 一 些 长 方形 的 积木 组 成 。 使 用 Kinect 时 ,人 类 示 教 者 控制 Baxter 机 器 人 拉 下 一 个 积 
木 ,并 控制 机 器 人 的 手臂 回 到 原来 的 位 置 , 如 图 13. 21 所 示 。 重 复 执行 该 动作 多 次 ,同时 记录 
机 器 人 在 此 过 程 中 的 关节 角度 。 

由 于 每 次 示 教 的 仿真 时 间 不 同 ,实验 中 记录 的 数据 长 度 也 不 相同 。 这 里 通过 随机 插值 保 
证 每 次 实验 数据 长 度 相 同 。 经 过 数据 处 理 , 得 到 一 个 142X3 维 的 样本 ,再 经 过 DTW 处 理 , 得 
到 一 组 维度 为 317X3 的 数据 ,如 图 13. 22 所 示 。 由 图 可 以 看 出 ,每 组 数据 之 间 的 差异 很 大 。 
利用 这 些 处 理 后 的 关节 角度 对 机 器 人 进行 控制 ,机 器 人 可 以 复 现 相同 的 轨迹 ,证 明 该 方法 不 影 
响 机 器 人 轨迹 。 
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图 13. 21 人 类 示范 者 用 Kinect 传感器 控制 Baxter 机 器 人 将 工件 推倒 


Sl/rad 


时 间 / 步 


El/rad 


时 间 / 步 


图 13.22 四 个 随机 选择 的 训练 样本 


2. 基于 RBF 神经 网 络 的 学 习 算法 

基于 RBF 神经 网 络 的 算法 是 在 MATLAB 中 实现 的 。 经 过 测试 ,发 现 最 佳 隐藏 层 节点 数 
为 12, 即 当 网 络 的 隐 层 有 12 个 节点 时 准确 性 最 高 。 使 用 RBF 神经 网 络 训练 三 组 输入 样本 ,得 
到 相应 的 输出 数据 ,机 器 人 对 应 关节 角度 的 值 如 图 13. 23 所 示 。 

在 另 一 个 实验 场景 中 ,Baxter 机 器 人 将 工件 从 一 个 传送 带 送 到 另 一 个 传送 带 。 用 实际 的 
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图 13.23 图 (a)、(b)、(c) 分 别 为 So、S1 和 El 的 关节 角度 ; (d) 为 Baxter 机 器 人 的 机 械 臂 在 
笛 卡 儿 空间 中 向 下 移动 的 轨迹 ; 参考 值 是 RBF 神经 网 络 的 输出 ,实际 值 记 录 的 
是 真实 的 Baxter 机 器 人 关节 和 角 


Baxter 机 器 人 对 基于 RBF 神经 网 络 学 习 算 法 的 有 效 性 进行 。 将 由 RBF 神经 网 络 生 成 的 轨迹 
直接 发 送 给 机 器 人 ; 同时 ,每 输入 一 组 关节 角度 ,都 记录 下 Baxter 机 器 人 关节 角度 的 实际 值 ， 
发 送 的 关节 角度 值 如 图 13. 23 所 示 。 可 以 看 出 ,实际 的 Baxter 机 器 人 可 以 复 现 由 RBF 神经 
网 络 训 练 得 到 的 轨迹 。 记 录 的 关节 角度 的 实际 值 如 图 13. 24 所 示 。 
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图 13.24 真实 的 Baxter 机 器 人 关节 角度 
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此 外 ,由 图 13. 22 和 图 13. 23 可 知 ,Baxter 机 器 人 的 轨迹 主要 与 SO 和 S1 有 关 。 在 机 械 臂 
向 下 运动 期 间 ,S0 的 最 小 值 近似 等 于 0.4,S1 的 最 大 值 近似 等 于 0.55。 因 此 ,如 图 13. 25 所 示 ， 
机 器 人 可 以 通过 基于 HRI 的 遥 操 作 学 习 移 动 轨迹 ,基于 RBF 神经 网 络 的 学 习 方法 可 以 学 习 
并 获得 轨迹 的 主要 特征 ,并且 它 在 重复 性 的 任务 中 表现 良好 。 


i 
图 13.25 实验 结果 3: 通过 RBF 神经 网 络 的 学 习 训 练 ,Baxter 机 器 人 在 V-REP 的 


仿真 环境 中 和 实际 的 物理 环境 中 都 可 以 复 现 人 类 示范 者 的 动作 


13.3 基于 零 空 间 的 机 器 人 柔顺 控制 


本 节 将 介绍 一 种 零 空 间 控制 器 ,以 实现 宛 余 机 器 人 的 柔顺 和 运动 行为 ,最 终 实现 安全 的 人 机 
人 
协同 机 器 人 的 设计 通常 满足 合 规 性 和 灵活 性 ,以 保证 人 机 交互 安全 ,这 是 人 机 器 人 交互 中 
最 重要 的 问题 。 虽 然 许 多 精密 的 机 制 ( 如 可 变 刚度 执行 器 和 系列 弹性 执行 器 ) 在 设计 时 可 用 于 
提高 机 器 人 的 灵活 性 ,但 如 何 设计 一 种 有 效 的 机 器 人 控制 方案 以 满足 人 -机 器 人 在 复杂 的 环境 
中 进行 物理 交互 仍然 是 一 个 巨大 的 挑战 。 

为 了 确保 人 机 协作 的 安全 ,人 们 设计 了 各 种 控制 框架 。 无 碰撞 控制 器 设计 用 于 避免 与 障 
碍 物 和 附近 人 类 的 可 能 碰撞 。 通 过 使 用 与 视觉 相机 信息 和 机 器 人 运动 学 相 结合 的 共享 控制 吉 
实现 自动 避 障 行为 ; 对 于 使 用 最 小 加 速度 范 数 和 基于 不 等 式 判 据 的 宛 余 机 器 人 ,可 以 使 用 联 
合 加 速度 级 避免 碰撞 控制 器 。 然 而 ,考虑 到 人 -机 器 人 共存 任务 的 需求 , 当 机 需 人 与 人 交互 其 
至 与 人 接触 时 ,更 期 望 执行 柔顺 的 机 右 人 控制 。 

对 于 宛 余 机 器 人 , 即 关 节 空 间 的 自由 度 (DOF) 大 于 任务 空间 中 的 自由 度 , 零 空间 阻抗 控 
制 对 于 实现 柔顺 控制 性 能 是 有 用 的 。 当 外 部 交互 作用 于 机 器 人 身体 时 ,这 种 柔顺 行为 是 通过 
将 相互 作用 力 投射 到 任务 空间 的 零 空 间 而 不 影响 主要 任务 来 实现 的 。 众 所 周知 ,阻抗 控制 方 
法 有 助 于 实现 主动 机 器 人 柔顺 控制 。 阻 抗 控制 器 通常 设计 用 于 控制 与 环境 相互 作用 的 机 器 人 
末端 执行 器 。 然 而 , 它 也 可 以 用 于 关节 空间 以 改善 机 器 人 身体 的 柔顺 性 。 通 过 组 合 零 空 间 控 
制 和 阻抗 控制 ,设计 的 零 空 间 阻 抗 控 制 器 用 于 加 速度 的 优先 级 控制 。 

应 该 注意 的 是 ,施加 在 机 器 人 上 的 外 部 力矩 不 容易 测量 。 为 了 在 不 干扰 主要 任务 的 情况 
下 保证 柔顺 的 阻抗 控制 性 能 ,需要 估计 并 适当 地 消除 外 力 。 然 而 ,在 一 些 应 用 中 ,期 望 实现 预 
定义 的 跟踪 性 能 。 本 节 将 介绍 一 种 使 用 规定 的 性 能 控制 器 来 控制 机 器 人 末端 执行 器 ,从 而 保 
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证 主要 任务 的 瞬 态 跟踪 性 能 。 通 过 使 用 零 空间 控制 法 则 确保 机 器 人 运动 的 柔顺 行为 ,通过 该 
空间 控制 法 可 以 将 作用 在 主要 任务 上 的 外 部 力矩 投射 到 零 空 间 ,并 且 可 以 减 小 外 力 的 影响 。 


13.3.1 系统 描述 
.系统 描述 
N 连 杆 机 械 臂 的 动力 学 可 以 描述 为 : 
M(lg)qgi+C(g,90)9+G(gq)+ry =r (13. 49) 
其 中 ,g 表示 关节 角度 ,M (gq) 是 惯性 /质量 矩阵 ,C(g ,9) 是 科 里 奥 利 /离心 矩阵 ,G (gq) 是 重力 
矢量 ,r 是 要 设计 的 关节 控制 力矩 ,rw 是 施加 在 接头 上 的 外 部 力矩 , 它 可 能 是 由 环境 或 未 模型 
干扰 的 相互 作用 引起 的 。 
关节 空间 阻抗 控制 可 以 描述 为 : 
Mr(gs = + BrlGs —6) + Ry =Y) = (13. 50) 
其 中 ,qs 表示 所 需 的 关节 轨迹 ,Mu ER ”BE 有 R ”和 天, ER ”分 别 是 所 需 的 惯性 矩阵 、 
阻尼 和 矩阵 和 刚度 矩阵 ,它们 均 为 正定 矩阵 。 
n 自由 度 (DOF) 宛 余 机 器 人 ,可 以 使 用 以 下 运动 方程 来 描述 末端 执行 器 速度 和 关节 速度 
之 间 的 关系 : 
z=J(g)g (13.:51) 
其 中 ,zz 是 任务 空间 速度 的 向 量 ,J (gq) 表示 相对 于 g 的 雅 可 比 和 矩阵 。 
对 于 宛 余 机 器 人 , 即 关节 空间 的 自由 度 大 于 任务 空间 的 自由 度 , 因 其 雅 可 比 和 矩阵 的 行 数 小 于 
列 数 , 所 以 不 是 方形 矩阵。 这 意味 着 ,对 于 给 de 之 ,9 的 道 解 可 能 不 是 唯一 的 。 


因此 ,可 以 后 空间 控制 的 一 般 道 解 来 实现 ,如 下 
6 一 Ji 之 十 No (13. 52) 
其 中 ,三 (go) 是 Jo) 的 道 解 ,N 表示 零 空 间 和 矩阵 ， rp 
N=I—JiJ (13. 53) 


2. 联合 空间 分 解 
采用 联合 空间 分 解 算法 描述 元 余 机 械 臂 的 运动 。 辅 助 矩阵 Z(o)、 辅 助 矢量 和 g 之 间 的 
关系 可 以 表示 为 : 
dg =Ng, =Z(g)A (13. 54) 
其 中 ,Z(g) 满 足 J(g)Z(g) 二 0,4 为 > 维 速度 矢量 。》 的 一 般 解 可 以 通过 使 用 惯性 加 权 广 义 
道 获 得 


A=Z*(g)g =(Z(g)'M(g)Z(g)) 'Z(g)'M(g) (13.55) 
则 有 : 
二 J(g) 
= ;3 =|( )s (13. 56) 
全 EE eS 
因为 是 满 秩 ,所 以 Jc(gq) 是 非 奇异 矩阵 。J (9g) 的 倒数 可 以 通过 下 式 计 算得 到 : 
Jc (0 = [Ty "Ztg)] (13. 57) 


其 中 ,J* (gq) 二 MJTIM 1JT) 1!。 
则 式 (13. 56) 和 式 (13. 57) 可 以 实现 关节 速度 的 分 解 : 
6 一 之 十 ZX (13. 58) 
3. 加 速 控 制 
为 了 在 任务 空间 和 关节 空间 中 控制 机 器 人 ,选择 如 下 控制 律 ; 


384 者 上 机 器 人 控制 一 运动 学 、 控 制 器 设计 、 人 机 交互 与 应 用 实例 


r=M(g)d. +C(g,9)9 |G(g) tr (13. 59) 
选择 9 为: 
j=2y + MI Bud + Kd —r,) 13, 60) 
其 中 ,Gg 二 gy 一 g。 通 过 选择 惯性 矩阵 M, 为 Mj 二 M (gq), 结 合式 (13. 60), 式 (13. 59) 可 以 改 
写 为 : 站 


tr=M(g)gy +Bg+KJg+C(g,9Y9+G(g) (13. 61) 
假设 外 力 rj 可 测量 得 到 ,由 式 (13. 59) 、 式 (13. 61) 和 式 (13. 52),g。 可 以 设计 为 : 
de =I (2 j++ NG 十 MT (Bad + Kd — ra)) (13. 62) 
其 中 ,zx。 是 命令 的 任务 空间 加 速 。 闭 环 机 器 人 系统 可 以 表示 成 如 下 形式 : 
t=, —JM rs (13. 63) 
NG 二 MI (BJ +Kyg9) — Mr,)=0 (13. 64) 


基于 式 (13. 63) 和 式 (13. 64) ,任务 空间 行为 与 零 空间 动态 分 离 。 换 名 话说 ,可 以 将 任务 空 
间 分 配 到 末端 执行 器 坐标 ,然后 在 任务 空间 的 零 空间 中 实现 关节 运动 调节 。 这 样 ,不 仅 可 以 控 
制 机 器 人 的 末端 执行 器 ,还 可 控制 机 器 人 本 体 的 运动 。 基 于 式 (13.56) 和 式 (13. 58) ,可 以 设计 
如 下 加 速 层面 的 控制 器 : 
A (13. 65) 
其 中 ,。 是 零 空 间 加 速度 的 虚拟 控制 输入 。 
由 于 二 多 #0 十 Z#d ,可 得 如 下 闭环 零 空 间 动力 学 方程 ; 
i=i. 一 Z#NM zr, (13. 66) 
4. 规定 性 能 控制 
引入 规定 性 能 控制 (Prescriptive Performance Control，PPC) 技 术 来 调节 机 器 人 在 任务 空 
间 中 的 控制 性 能 。 将 跟踪 误差 定义 为 e= 二 x 一 x ,其 中 x 是 任务 空间 轨迹 ,zy 是 需要 跟踪 的 
轨迹 。 
基于 规定 的 运动 性 能 引入 以 下 函数 : 


p=(p0 —pw)e "二 po (13. 67) 
人 
| #2 
l+e” 
R,;(x)= (13..68) 
(= 若 e。 二 0 
e 
1 十 er 


其 中 ,oo .pa 和 6 是 规定 参数 ,以 调节 瞬 态 控制 性 能 。 
基于 式 (13. 67) 和 式 (13. 68) ,误差 变换 函数 可 以 设计 如 下 : 
e; 一 DCG)R;(CE; (CD)) (13. 69) 
其 中 ,é, 是 转换 后 的 误差 。 根 据 上 述 误差 变换 设计 , 当 ee 二 0 时 ,我 们 得 到 一 6 二 R, 二 1; 当 e 宇 0 
时 ,一 一 1<R;<<8。 因 此 可 以 推导 出 ,如 果 &; 是 有 界 的 ,那么 一 6, 二 e; 二 ol(e0) 和 一 o<e< 
6,(e 二 0)。 这 意味 着 。 可 用 于 指定 跟踪 误差 的 上 限 和 下 限 , 当 &;, 有 界 时 ,如 图 13. 26 所 示 。 
根据 式 (13. 68) 和 式 (13. 69) ,可 以 获得 &, 的 解 
é€;(1)=L,(e;(t)/0) (13.70) 
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跟踪 误差 /m 


图 13.26 性 能 控制 


其 中 
(mn 车 2 宇 0 
~ 
LC 0 Ch3. 71) 
TX -二 
in 一) > 
对 式 (13. 70) 求 导 可 得 
LR (Et 
é,(1)=——— (p(t —p(t)e) (C18: 72) 
t 2 
CY 
。 i148 。 1 十 $ 
一 一 一 二 9 i o 
其 中 ,L (zx) m0) 


基于 上 述 分 析 , 可 知 控制 的 目标 是 保证 &, 的 有 界 性 ,从 而 可 以 实现 规定 的 跟踪 性 能 。 

13.3.2 控制 器 设计 

控制 目标 是 保证 未 端 执行 器 的 运动 控制 性 能 ,用 于 跟踪 期 望 的 轨迹 ,同时 允许 机 器 人 本 体 
在 零 空间 中 执行 柔顺 行为 。 控 制 器 的 总 体 结构 如 图 13. 27 所 示 。 


图 13.27 控制 器 的 总 体 结构 


1. 基于 主要 任务 的 控制 设计 
定义 机 器 人 的 跟踪 残 差 为 


2 一 之 一 1 (13,73) 

其 中 ,vs 是 辅助 控制 器 。 
考虑 到 式 (13. 63) ,为 了 跟踪 具有 规定 性 能 的 期 望 轨迹 x ,设计 如 下 加 速度 控制 律 
六 = Me (ee FE = (13.74) 


C 
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其 中 , 玉 ， 是 正定 矩阵 ,fu 是 外 部 转 矩 的 估计 ,M, 二 (JM !J") :是 任务 空间 中 的 惯性 矩阵 ， 
C, 是 任务 空间 科 里 奥 利 和 离心 矩阵 。 
联 立 式 (13.74) 和 式 (13. 63) 可 得 : 


ut WE (te Mr (13.75) 
假定 wu =kips; 十 a 十 ep /p(t) ,其 中 人 是 正 控制 增益 。 因 此 ,只 要 z 收敛 到 零 ,就 可 以 
实现 上 的 有 界 性 。 那 么 , 式 (13.72) 可 以 通过 vv = 二 [vy1，… ,vy j 的 定义 表示 为 : 


有 LER Ce (2 
é,(t)=—kIL(R,(€,(1)))é8; 十 一 ooo, (13.76) 


(1) 
式 (13.75) 两 边 同 时 乘 以 M, ,有 
RM 之 十 J#Trs -Mi 一 Ce + K+ tI Cl 77) 
使 用 J* (gq) 二 M 1J'(JM !'J') ,可 以 将 闭环 任务 空间 动态 描述 为 
Lé 和 JsTe， (13. 78) 


M,2=—(C, +K,)x 二 二 
其 中 ,zs 一 2 一 rd。 
基于 自 适应 扰动 观测 器 ,用 于 估计 外 部 力矩 ry 的 自 适应 律 设 计 为 
t=—T J*z (13.79) 
其 中 , 为 正定 矩阵 。 
2. 零 空 间 任务 的 控制 设计 
一 般 来 说 , 零 空间 速度 不 能 被 积分 ,因此 ,不 能 为 系统 定义 零 空 间 位 置 误差 。 或 者 ,可 以 
基于 式 (13. 66) 和 关节 空间 误差 定义 零 空 间 命令 加 速度 : 
i=i, +Mi!((C, +K)i+2'K,d) (13. 80) 
其 中 ,K，、 和 K, 分 别 是 要 设计 的 正定 矩阵 和 对 称 和 矩阵 。 并 且 Mi = 二 Z MZ 是 零 空 间 的 惯性 
矩阵 ,而 C, 是 零 空 间 科 里 奥 利 和 离心 矩阵 。 
基于 式 (13. 66) 和 式 (13. 80) ,闭环 零 空 间 动力 学 可 以 表示 为 
Wt EOI TZ R=2Tr, (13. 81) 
3. 稳定 性 分 析 
考虑 一 个 由 式 (13. 49) 描 述 的 动力 学 宛 余 控制 器 ,任务 空间 闭环 动力 学 和 零 空 间 闭环 动力 
学 分 别 由 式 (13. 63) 和 式 (13. 66) 描 述 ,控制 式 (13. 74) 和 式 (13. 55) 可 以 保证 机 器 人 用 规定 的 
零 空 间 行为 跟踪 所 需 的 轨迹 ,而 上 6. 和 w 可 以 渐 近 收敛 到 零 , 并 且 从 不 违反 规定 的 运动 约束 。 
另外 ,估计 的 转 矩 是 有 界 的 ,闭环 系统 是 稳定 的 。 
证 明 : 考虑 如 下 李 雅 普 诺 夫 函数 


伙 一 676 2 
对 式 (13. 82) 求 导 , 可 得 


Vv _é§€ LL(€)z(i) 


= 和 13. 83) 
; te K&L (8)E ( 


其 中 ,LL (8)=diag(Li (RI CE)) ,LL, (R, (Ce(GD))))。 
则 下 面 的 李 雅 普 诺 夫 函 数 可 设计 为 
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A 


> =7z"M, z 十 二 了 ?rev (13. 84) 
其 中 ,了 是 设计 的 正定 矩阵 。 
对 式 (13. 84) 求 导 , 有 
V, 三 Z&TM 之 十 Fe Ms 十 zt， (13. 85) 


将 式 (13.78) 代 入 式 (13. 85), 可 得 


V =z7(— (C, +K,)z I | 十 za (13. 86) 


后 ,将 式 (13. TO, 86) ,考虑 M 一 2C，, 是 一 个 偏 斜 对 称 和 矩阵 , 则 有 


Vs 一 一 zIK zx 十 J#TPy — tJ zt kt (13. 87) 
结合 式 (13. 83) 和 式 (13. 87) 可 得 
Vi 上 +V =—zTK,z+zT 和 Ee 
=—z K,z— Kié'L(e)e (13. 88) 
由 于 不 等 式 & 人 TL(€)é 宇 2 上 (1) | 2/1 十 8 可 以 根据 LC) 的 定义 推导 出 来 ,因此 可 得 
2K; | 6Gz) | 


V=Vi +V, =—z'K,xz < (13. 89) 


根据 LaSalle 的 不 变性 原理 ,可 以 证 明 闭 环 系统 的 稳定 性 , 即 > 和 & 收敛 到 零 ,并 且 z, 一 
致 有 界 。 

为 进一步 证 明 零 空间 闭环 系统 在 子 集 C 二 {9,4,z4 ,6 一 0,z 一 0} 条 件 下 的 稳定 性 ,考虑 如 
下 李 雅 普 诺 夫 函 数 Ve: 


ve = ML + Kd 二 ?zz (13. 90) 
式 (13. 90) 在 C 中 是 正定 的 。 对 式 (13. 90) 求 导数 : 
Vo 一 jTM Cg 十 iT (gHa KG + eTE, (13. 91) 


将 式 (18.81) 代 大 式 《13:391) 有 

Ve = (一 (C, FR -ZRKGFZ'r,) + A (gq)A Ha KG +rITE, (13.92) 

由 于 M; 一 2C; 是 一 个 偏 斜 对 称 矩 阵 , 考 虑 到 二 0, 且 z=0 和 4 一 ZA, 则 有 
Vce=—iTKA—A Zr (13. 93) 

根据 Z 的 定义 ,可 得 ZTrj = 二 0。 那 么 


Ver 一 一 MTKiA 雪 0 (13. 94) 
根据 Lasalle 定理 , 子 集 C 中 的 零 空间 系统 是 浙 近 稳定 的 ,系统 状态 收敛 到 由 {z= 二 0,&==0， 
1 二 0,Z TK,9 二 0} 定 义 的 不 变 集 。 
基于 上 述 稳 定性 分 析 , 可 以 得 知 : 系统 状态 &.z 和 】 是 渐 近 稳定 的 。 因 此 ,可 以 保证 任务 
空间 中 规定 的 运动 性 能 和 关节 空间 中 的 柔顺 行为 。 
13.3.3 仿真 
为 证 明 上 述 算法 的 有 效 性 ,基于 三 自由 度 机 器 臂 进行 仿真 ,如 图 13. 28 所 示 。 机 器 人 每 个 
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连 杆 的 运动 学 和 惯性 参数 见 表 13. 2。 机 械 臂 的 动力 学 描述 如 下 : 
M(g)g+C(g,9)9 + G(g)++ry =r (13. 95) 
其 中 ,机 械 辟 惯性 矩阵 M(g) 和 科 里 奥 利 和 矩阵 CClg ,9 ) 可 描述 为 
Mi bs x | 
M(g)= IM, M,, 
Ms Ma Mss 


Ci Cy Cs | Gl 
Clg,0)= IC Co Co G(g)= IG 
C31 Ca C3 Cial 


图 13.28 三 自由 度 平面 机 器 人 
表 13.2 机 器 人 参数 


参 数 连 杆 3 
连 杆 长 度 (m) ls=0.7 
连 杆 质量 (kg) ma=2 
惯性 力 (kgm2 ) Ts 一 50 
连 杆 距离 (m) 0 


机 器 人 按 顺 序 绕 圆圈 达到 6 个 设 定点 ,如 图 13. 29 所 示 。 如 图 所 示 , 当 机 器 人 达到 一 个 设 
定点 时 ,外 力 将 施加 在 机 器 人 上 ,机 器 人 将 在 2s 内 停留 在 目标 点 上 。 然 后 机 器 人 移动 到 下 一 
个 设 定点 。 初 始 位 置 设置 为 zx (0) 二 [0.4,2], 速 度 为 (0) 二 [0,0j], 控 制 增益 选 为 正定 矩阵， 
即 K, 二 diag{50,50,50},Ki 为 diag{10,10,10); 干扰 观察 者 的 增益 选择 为 卫 = diag{0. 1， 
0a150;1} 3 外力 选 为 '€ 三 [一 10, 一 20,10]; : 


5 


YY” ed | 


一 相位 轨迹 
外 力 -一 一 I 


起 始点 


l;3 
0504 -03 0201 0 ‘1 02°03 0.4° 05 


Xx/m 


图 13.29 机 器 人 控制 的 相位 轨迹 
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相位 轨迹 如 图 13. 29 所 示 ,可 以 看 到 机 器 人 已 成 功 到 达 每 个 目标 点 。 为 了 清晰 地 展示 该 
方法 的 优点 ,图 13. 30 一 图 13. 32 给 出 了 机 器 人 在 每 个 阶段 保持 目标 点 的 跟踪 性 能 。z 方向 
和 y 方向 的 跟踪 误差 如 图 13. 30 和 图 13. 31 所 示 。 由 图 可 知 ,虽然 应 用 了 外 力 ,但 机 器 人 只 需 
稍微 移动 即 可 保持 目标 点 。 此 外 ,跟踪 误差 保持 在 零 附近 ,使 得 不 违反 规定 的 边界 。 相 比 之 
下 , 当 采 用 外 力 时 ,在 PID 控制 器 下 可 以 观察 到 机 器 人 明显 的 移 位 。 控 制 输 入 如 图 13. 32 所 
示 ，, 可 以 看 到 控制 输入 能 够 快速 调整 外 力 , 机 器 人 能 够 有 效 地 保持 所 需 的 设 定点 。 这 些 实验 结 
果 证 明了 上 述 控制 器 的 有 效 性 。 


0.3 ' 
1 一 一 零 空间 控制 器 
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图 13.30 x 方向 的 跟踪 误差 


0 4 8 10 


6 
跟踪 误差 Am 
图 13.31 y 方向 的 跟踪 误差 


控制 输入 A 
图 13.32 机 械 辟 的 控制 输入 


13.4 本 章 小 结 


本 章 介 绍 了 机 器 人 控制 技术 在 人 机 交互 . 示 教 和 控制 方向 的 一 些 综合 应 用 。 机 器 人 控制 
技术 的 应 用 还 有 很 多 ,这 里 只 介绍 个 别 较为 先进 主流 的 应 用 , 感 兴趣 的 读者 可 在 此 基础 上 进行 
深入 研究 。 
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